diff --git a/.gitignore b/.gitignore
index 94d4918bdec0d0d02df589c529d938e6c87d72d4..77906100d4ae252720e456622103b13d8c6371e5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -32,4 +32,7 @@ src/examples/map_apriltag_save.yaml
 \.vscode/
 build_release/
 .clangd
-wolf.found
+wolfcore.found
+/wolf.found
+.ccls*
+compile_commands.json
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index dbf139b13a33b4b14ec3125cca2621f6c9e47381..1adcb74ceb72d69e5108c5fb639a45c7d2a0e1ee 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -67,7 +67,10 @@ wolf_build_and_test:
     - mkdir -pv build
     - cd build
     - ls # we can check whether the directory was already full
-    - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+    - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
     - make -j$(nproc)
     - ctest -j$(nproc)
+    # run demos
+    - ../bin/hello_wolf
+    - ../bin/hello_wolf_autoconf
     - make install
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9112af5f16d8a84404e203361c568563ccb88dee..246dffbc6f04c9054d91d8f09fb0daadf5052d98 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,6 @@
 # Pre-requisites about cmake itself
+#Start WOLF build
+MESSAGE("Starting WOLF CMakeLists ...")
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 if(COMMAND cmake_policy)
@@ -10,32 +12,16 @@ SET(CMAKE_MACOSX_RPATH 1)
 
 
 # The project name
-PROJECT(wolf)
-
-#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result)
-#IF(result)
-#  SET(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR})
-#ENDIF()
-#message(STATUS "Binary path : " ${CMAKE_BINARY_DIR})
-#
-
-#SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-#SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-#
-# We'll set the install prefix only is it's empty
-# which shouldn't be the case ...
-#string(COMPARE EQUAL "${CMAKE_INSTALL_PREFIX}" "" result)
-#IF(result)
-  # This path is actually default on linux
-#  SET(CMAKE_INSTALL_PREFIX /usr/local)
-#ENDIF()
-#message(STATUS "Installation path : " ${CMAKE_INSTALL_PREFIX})
+PROJECT(core)
+set(PLUGIN_NAME "wolf${PROJECT_NAME}")
 
+# Paths
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
 SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
 
+# Build type
 IF (NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE "DEBUG")
 ENDIF (NOT CMAKE_BUILD_TYPE)
@@ -64,12 +50,14 @@ if(UNIX)
   set(CMAKE_CXX_FLAGS
     "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
 endif(UNIX)
+
+# Options
 IF(NOT BUILD_TESTS)
   OPTION(BUILD_TESTS "Build Unit tests" ON)
 ENDIF(NOT BUILD_TESTS)
 
 IF(NOT BUILD_DEMOS)
-  OPTION(BUILD_DEMOS "Build Demos" OFF)
+  OPTION(BUILD_DEMOS "Build Demos" ON)
 ENDIF(NOT BUILD_DEMOS)
 
 IF(NOT BUILD_DOC)
@@ -80,10 +68,7 @@ IF(PROFILING)
   add_definitions(-DPROFILING)
   message("Compiling with profiling options...")
 ENDIF(PROFILING)
-#############
-## Testing ##
-#############
-#
+
 if(BUILD_TESTS)
     # Enables testing for this directory and below.
     # Note that ctest expects to find a test file in the build directory root.
@@ -92,18 +77,6 @@ if(BUILD_TESTS)
     enable_testing()
 endif()
 
-#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
 IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
   set(_WOLF_DEBUG true)
 ENDIF()
@@ -118,12 +91,19 @@ IF(BUILD_DEMOS OR BUILD_TESTS)
 ENDIF(BUILD_DEMOS OR BUILD_TESTS)
 
 
-#find dependencies.
+#START_SRC --------------------------------------------------------------------------------------------------------------------------------
 
+#CMAKE modules
+
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH})
+
+
+#find dependencies.
 
 FIND_PACKAGE(Threads REQUIRED)
 
-FIND_PACKAGE(Ceres REQUIRED) #Ceres is not required
+FIND_PACKAGE(Ceres REQUIRED) #Ceres is required
 
 FIND_PACKAGE(Eigen3 3.3 REQUIRED)
 if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
@@ -160,8 +140,10 @@ ELSE (SPDLOG_INCLUDE_DIR)
  MESSAGE(FATAL_ERROR "Could not find spdlog")
 ENDIF (SPDLOG_INCLUDE_DIR)
 
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES("include")
+
+# Includes
+INCLUDE_DIRECTORIES("include") # In this same project
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 
@@ -291,6 +273,7 @@ SET(HDRS_TREE_MANAGER
   include/core/tree_manager/factory_tree_manager.h
   include/core/tree_manager/tree_manager_base.h
   include/core/tree_manager/tree_manager_sliding_window.h
+  include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
   )
 SET(HDRS_YAML
   include/core/yaml/parser_yaml.h
@@ -380,6 +363,7 @@ SET(SRCS_SOLVER
   )
 SET(SRCS_TREE_MANAGER
   src/tree_manager/tree_manager_sliding_window.cpp
+  src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
   )
 SET(SRCS_YAML
   src/yaml/parser_yaml.cpp
@@ -392,16 +376,17 @@ SET(SRCS_YAML
 IF (Ceres_FOUND)
     SET(HDRS_WRAPPER
       #ceres_wrapper/qr_manager.h
-      include/core/ceres_wrapper/ceres_manager.h
       include/core/ceres_wrapper/cost_function_wrapper.h
       include/core/ceres_wrapper/create_numeric_diff_cost_function.h
       include/core/ceres_wrapper/local_parametrization_wrapper.h
+      include/core/ceres_wrapper/iteration_update_callback.h
+      include/core/ceres_wrapper/solver_ceres.h
       include/core/solver/solver_manager.h
       include/core/solver_suitesparse/sparse_utils.h
       )
     SET(SRCS_WRAPPER
       #ceres_wrapper/qr_manager.cpp
-      src/ceres_wrapper/ceres_manager.cpp
+      src/ceres_wrapper/solver_ceres.cpp
       src/ceres_wrapper/local_parametrization_wrapper.cpp
       src/solver/solver_manager.cpp
       )
@@ -420,7 +405,7 @@ IF (Suitesparse_FOUND)
 ENDIF(Suitesparse_FOUND)
 
 # create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
+ADD_LIBRARY(${PLUGIN_NAME}
   SHARED
   ${SRCS}
   ${SRCS_BASE}
@@ -449,35 +434,30 @@ ADD_LIBRARY(${PROJECT_NAME}
 # ====================  
 if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
   message(STATUS "Using C++ compiler clang")
-  target_compile_options(${PROJECT_NAME} PRIVATE -Winconsistent-missing-override)
+  target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
   # using Clang
 elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   message(STATUS "Using C++ compiler gnu")
-  target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override)
+  target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
   # using GCC
 endif()
 
 #Link the created libraries
 #=============================================================
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
 IF (Ceres_FOUND)
-    TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
+    TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
 ENDIF(Ceres_FOUND)
 
-# IF (GLOG_FOUND)
-#     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-# ENDIF (GLOG_FOUND)
-#check if this is done correctly
-
 IF(BUILD_TESTS)
-  MESSAGE("Building tests.")
+  MESSAGE(STATUS "Will build tests.")
   add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
 IF(BUILD_DEMOS)
   #Build demos
-  MESSAGE("Building demos.")
+  MESSAGE(STATUS "Will build demos.")
   ADD_SUBDIRECTORY(demos)
 ENDIF(BUILD_DEMOS)
 
@@ -485,12 +465,12 @@ ENDIF(BUILD_DEMOS)
 #install library
 
 #=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
+INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib/iri-algorithms
   ARCHIVE DESTINATION lib/iri-algorithms)
 
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
 #install headers
 INSTALL(FILES ${HDRS_CAPTURE}
    DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
@@ -535,13 +515,13 @@ INSTALL(FILES ${HDRS_WRAPPER}
 INSTALL(FILES ${HDRS_YAML}
   DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
 
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
+FILE(WRITE ${PLUGIN_NAME}.found "")
+INSTALL(FILES ${PLUGIN_NAME}.found
   DESTINATION include/iri-algorithms/wolf/plugin_core)
 
 #install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
-               "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
+configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
+               "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" @ONLY)
 
 configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
                "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
@@ -549,14 +529,15 @@ configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
 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(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
 
 INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 
-export(PACKAGE ${PROJECT_NAME})
+export(PACKAGE ${PLUGIN_NAME})
 
 #-END_SRC --------------------------------------------------------------------------------------------------------------------------------
+
 FIND_PACKAGE(Doxygen)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
@@ -599,17 +580,4 @@ ELSE(UNIX)
     )
 ENDIF(UNIX)
 
-IF (UNIX)
-  SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
-  SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
-  SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
-  SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
-  SET(CPACK_GENERATOR "DEB")
-  SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
-  SET(CPACK_SET_DESTDIR "ON")  # Necessary because of the absolute install paths
-  INCLUDE(CPack)
-ELSE(UNIX)
-  ADD_CUSTOM_COMMAND(
-    COMMENT "packaging only implemented in unix"
-    TARGET  uninstall)
-ENDIF(UNIX)
+
diff --git a/README.md b/README.md
index ad5473fec99242beacf3889f0818e3f4cd93caa7..c1e960469ccdcaf2b9918905b7e10536c9e9af59 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
 WOLF - Windowed Localization Frames
 ===================================
 
-For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
\ No newline at end of file
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
\ No newline at end of file
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
index d44ea8903d4c5a72af468d603dc1d8e5a6bbf542..9e969786089ca8ea3801be8b084c51a5782f09b5 100644
--- a/cmake_modules/FindEigen3.cmake
+++ b/cmake_modules/FindEigen3.cmake
@@ -1,263 +1,97 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
+# - Try to find Eigen3 lib
 #
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
 #
-# * Redistributions of source code must retain the above copyright notice,
-#   this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-#   this list of conditions and the following disclaimer in the documentation
-#   and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-#   used to endorse or promote products derived from this software without
-#   specific prior written permission.
+# Once done this will define
 #
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
 #
-# Author: alexs.mac@gmail.com (Alex Stewart)
+# This module reads hints about search locations from 
+# the following enviroment variables:
 #
+# EIGEN3_ROOT
+# EIGEN3_ROOT_DIR
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+  
+  # search first if an Eigen3Config.cmake is available in the system,
+  # if successful this would set EIGEN3_INCLUDE_DIR and the rest of
+  # the script will work as usual
+  find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
+
+  if(NOT EIGEN3_INCLUDE_DIR)
+    find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+        HINTS
+        ENV EIGEN3_ROOT 
+        ENV EIGEN3_ROOT_DIR
+        PATHS
+        ${CMAKE_INSTALL_PREFIX}/include
+        ${KDE4_INCLUDE_DIR}
+        PATH_SUFFIXES eigen3 eigen
+      )
+  endif(NOT EIGEN3_INCLUDE_DIR)
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
 
-# FindEigen.cmake - Find Eigen library, version >= 3.
-#
-# This module defines the following variables:
-#
-# EIGEN_FOUND: TRUE iff Eigen is found.
-# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
-# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
-# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
-# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
-# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
-# FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION: True iff the version of Eigen
-#                                            found was built & installed /
-#                                            exported as a CMake package.
-#
-# The following variables control the behaviour of this module:
-#
-# EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
-#                           then prefer using an exported CMake configuration
-#                           generated by Eigen over searching for the
-#                           Eigen components manually.  Otherwise (FALSE)
-#                           ignore any exported Eigen CMake configurations and
-#                           always perform a manual search for the components.
-#                           Default: TRUE iff user does not define this variable
-#                           before we are called, and does NOT specify
-#                           EIGEN_INCLUDE_DIR_HINTS, otherwise FALSE.
-# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
-#                          search for eigen includes, e.g: /timbuktu/eigen3.
-#
-# The following variables are also defined by this module, but in line with
-# CMake recommended FindPackage() module style should NOT be referenced directly
-# by callers (use the plural variables detailed above instead).  These variables
-# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
-# are NOT re-called (i.e. search for library is not repeated) if these variables
-# are set with valid values _in the CMake cache_. This means that if these
-# variables are set directly in the cache, either by the user in the CMake GUI,
-# or by the user passing -DVAR=VALUE directives to CMake when called (which
-# explicitly defines a cache variable), then they will be used verbatim,
-# bypassing the HINTS variables and other hard-coded search locations.
-#
-# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
-#                    include directory of any dependencies.
-
-# Called if we failed to find Eigen or any of it's required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
-macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
-  unset(EIGEN_FOUND)
-  unset(EIGEN_INCLUDE_DIRS)
-  unset(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION)
-  # Make results of search visible in the CMake GUI if Eigen has not
-  # been found so that user does not have to toggle to advanced view.
-  mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
-  # use the camelcase library name, not uppercase.
-  if (Eigen_FIND_QUIETLY)
-    message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
-  elseif (Eigen_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use no priority which emits a message
-    # but continues configuration and allows generation.
-    message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(EIGEN_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set EIGEN_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(EIGEN_FOUND)
-
-# -----------------------------------------------------------------
-# By default, if the user has expressed no preference for using an exported
-# Eigen CMake configuration over performing a search for the installed
-# components, and has not specified any hints for the search locations, then
-# prefer an exported configuration if available.
-if (NOT DEFINED EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION
-    AND NOT EIGEN_INCLUDE_DIR_HINTS)
-  message(STATUS "No preference for use of exported Eigen CMake configuration "
-    "set, and no hints for include directory provided. "
-    "Defaulting to preferring an installed/exported Eigen CMake configuration "
-    "if available.")
-  set(EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION TRUE)
-endif()
-
-if (EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION)
-  # Try to find an exported CMake configuration for Eigen.
-  #
-  # We search twice, s/t we can invert the ordering of precedence used by
-  # find_package() for exported package build directories, and installed
-  # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7)
-  # respectively in [1].
-  #
-  # By default, exported build directories are (in theory) detected first, and
-  # this is usually the case on Windows.  However, on OS X & Linux, the install
-  # path (/usr/local) is typically present in the PATH environment variable
-  # which is checked in item 4) in [1] (i.e. before both of the above, unless
-  # NO_SYSTEM_ENVIRONMENT_PATH is passed).  As such on those OSs installed
-  # packages are usually detected in preference to exported package build
-  # directories.
-  #
-  # To ensure a more consistent response across all OSs, and as users usually
-  # want to prefer an installed version of a package over a locally built one
-  # where both exist (esp. as the exported build directory might be removed
-  # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which
-  # means any build directories exported by the user are ignored, and thus
-  # installed directories are preferred.  If this fails to find the package
-  # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any
-  # exported build directories will now be detected.
-  #
-  # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which
-  # is item 5) in [1]), to not preferentially use projects that were built
-  # recently with the CMake GUI to ensure that we always prefer an installed
-  # version if available.
-  #
-  # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
-  find_package(Eigen3 QUIET
-                      NO_MODULE
-                      NO_CMAKE_PACKAGE_REGISTRY
-                      NO_CMAKE_BUILDS_PATH)
-  if (EIGEN3_FOUND)
-    message(STATUS "Found installed version of Eigen: ${Eigen3_DIR}")
-  else()
-    # Failed to find an installed version of Eigen, repeat search allowing
-    # exported build directories.
-    message(STATUS "Failed to find installed Eigen CMake configuration, "
-      "searching for Eigen build directories exported with CMake.")
-    # Again pass NO_CMAKE_BUILDS_PATH, as we know that Eigen is exported and
-    # do not want to treat projects built with the CMake GUI preferentially.
-    find_package(Eigen3 QUIET
-                        NO_MODULE
-                        NO_CMAKE_BUILDS_PATH)
-    if (EIGEN3_FOUND)
-      message(STATUS "Found exported Eigen build directory: ${Eigen3_DIR}")
-    endif()
-  endif()
-  if (EIGEN3_FOUND)
-    set(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION TRUE)
-    set(EIGEN_FOUND ${EIGEN3_FOUND})
-    set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}" CACHE STRING
-      "Eigen include directory" FORCE)
-  else()
-    message(STATUS "Failed to find an installed/exported CMake configuration "
-      "for Eigen, will perform search for installed Eigen components.")
-  endif()
-endif()
-
-if (NOT EIGEN_FOUND)
-  # Search user-installed locations first, so that we prefer user installs
-  # to system installs where both exist.
-  list(APPEND EIGEN_CHECK_INCLUDE_DIRS
-    /usr/local/include
-    /usr/local/homebrew/include # Mac OS X
-    /opt/local/var/macports/software # Mac OS X.
-    /opt/local/include
-    /usr/include)
-  # Additional suffixes to try appending to each search path.
-  list(APPEND EIGEN_CHECK_PATH_SUFFIXES
-    eigen3 # Default root directory for Eigen.
-    Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3
-    Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3
-
-  # Search supplied hint directories first if supplied.
-  find_path(EIGEN_INCLUDE_DIR
-    NAMES Eigen/Core
-    HINTS ${EIGEN_INCLUDE_DIR_HINTS}
-    PATHS ${EIGEN_CHECK_INCLUDE_DIRS}
-    PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
-
-  if (NOT EIGEN_INCLUDE_DIR OR
-      NOT EXISTS ${EIGEN_INCLUDE_DIR})
-    eigen_report_not_found(
-      "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
-      "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
-  endif (NOT EIGEN_INCLUDE_DIR OR
-    NOT EXISTS ${EIGEN_INCLUDE_DIR})
-
-  # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
-  # if called.
-  set(EIGEN_FOUND TRUE)
-endif()
-
-# Extract Eigen version from Eigen/src/Core/util/Macros.h
-if (EIGEN_INCLUDE_DIR)
-  set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
-  if (NOT EXISTS ${EIGEN_VERSION_FILE})
-    eigen_report_not_found(
-      "Could not find file: ${EIGEN_VERSION_FILE} "
-      "containing version information in Eigen install located at: "
-      "${EIGEN_INCLUDE_DIR}.")
-  else (NOT EXISTS ${EIGEN_VERSION_FILE})
-    file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
-
-    string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
-      EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
-    string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
-      EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
-
-    string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
-      EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
-    string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
-      EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
-
-    string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
-      EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
-    string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
-      EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
-
-    # This is on a single line s/t CMake does not interpret it as a list of
-    # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
-    set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
-  endif (NOT EXISTS ${EIGEN_VERSION_FILE})
-endif (EIGEN_INCLUDE_DIR)
-
-# Set standard CMake FindPackage variables if found.
-if (EIGEN_FOUND)
-  set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
-endif (EIGEN_FOUND)
-
-# Handle REQUIRED / QUIET optional arguments and version.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Eigen
-  REQUIRED_VARS EIGEN_INCLUDE_DIRS
-  VERSION_VAR EIGEN_VERSION)
-
-# Only mark internal variables as advanced if we found Eigen, otherwise
-# leave it visible in the standard GUI for the user to set manually.
-if (EIGEN_FOUND)
-  mark_as_advanced(FORCE EIGEN_INCLUDE_DIR
-    Eigen3_DIR) # Autogenerated by find_package(Eigen3)
-endif (EIGEN_FOUND)
\ No newline at end of file
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
deleted file mode 100644
index 74ede89df7d20ffdfe00d7cd669bce317fc501a9..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfConfig.cmake
+++ /dev/null
@@ -1,99 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolf_INCLUDE_DIRS
-    NAMES wolf.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-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_DIRS)
-
-FIND_LIBRARY(
-    wolf_LIBRARIES
-    NAMES libwolf.so libwolf.dylib
-    PATHS /usr/local/lib/iri-algorithms)
-IF(wolf_LIBRARIES)
-  MESSAGE("Found wolf lib: ${wolf_LIBRARIES}")
-ELSE(wolf_LIBRARIES)
-  MESSAGE("Couldn't find wolf lib")
-ENDIF(wolf_LIBRARIES)
-
-IF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
-   SET(wolf_FOUND TRUE)
- ELSE(wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
-   set(wolf_FOUND FALSE)
-ENDIF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
-
-IF (wolf_FOUND)
-   IF (NOT wolf_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf: ${wolf_LIBRARIES}")
-   ENDIF (NOT wolf_FIND_QUIETLY)
-ELSE (wolf_FOUND)
-   IF (wolf_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf")
-   ENDIF (wolf_FIND_REQUIRED)
-ENDIF (wolf_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolf_FOUND FALSE)
-  unset(wolf_INCLUDE_DIRS)
-  unset(wolf_LIBRARIES)
-
-  # Reset the CMake module path to its state when this script was called.
-  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
-  # FindPackage() use the camelcase library name, not uppercase.
-  if (wolf_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf- " ${REASON_MSG} ${ARGN})
-  else (wolf_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
-    # that prevents generation, but continues configuration.
-    message(SEND_ERROR "Failed to find wolf - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-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)
-
-# 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})
-
-#Eigen
-# FIND_PACKAGE(Eigen3 REQUIRED)
-# list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-
-if(NOT Eigen3_FOUND)
-  FIND_PACKAGE(Eigen3 REQUIRED)
-endif()
-if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
-  message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
-endif()
-list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-
-SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..b4df7e15bab20bc8d1090ffbd5428ac7a3ab5cd6
--- /dev/null
+++ b/cmake_modules/wolfcoreConfig.cmake
@@ -0,0 +1,99 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(
+    wolfcore_INCLUDE_DIRS
+    NAMES wolfcore.found
+    PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
+IF(wolfcore_INCLUDE_DIRS)
+  MESSAGE("Found wolf core include dirs: ${wolfcore_INCLUDE_DIRS}")
+ELSE(wolfcore_INCLUDE_DIRS)
+  MESSAGE("Couldn't find wolf core include dirs")
+ENDIF(wolfcore_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+    wolfcore_LIBRARIES
+    NAMES libwolfcore.so libwolfcore.dylib
+    PATHS /usr/local/lib/iri-algorithms)
+IF(wolfcore_LIBRARIES)
+  MESSAGE("Found wolf core lib: ${wolfcore_LIBRARIES}")
+ELSE(wolfcore_LIBRARIES)
+  MESSAGE("Couldn't find wolf core lib")
+ENDIF(wolfcore_LIBRARIES)
+
+IF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+   SET(wolfcore_FOUND TRUE)
+ ELSE(wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+   set(wolfcore_FOUND FALSE)
+ENDIF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+
+IF (wolfcore_FOUND)
+   IF (NOT wolfcore_FIND_QUIETLY)
+      MESSAGE(STATUS "Found wolf core: ${wolfcore_LIBRARIES}")
+   ENDIF (NOT wolfcore_FIND_QUIETLY)
+ELSE (wolfcore_FOUND)
+   IF (wolfcore_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find wolf core")
+   ENDIF (wolfcore_FIND_REQUIRED)
+ENDIF (wolfcore_FOUND)
+
+
+macro(wolfcore_report_not_found REASON_MSG)
+  set(wolfcore_FOUND FALSE)
+  unset(wolfcore_INCLUDE_DIRS)
+  unset(wolfcore_LIBRARIES)
+
+  # Reset the CMake module path to its state when this script was called.
+  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+  # FindPackage() use the camelcase library name, not uppercase.
+  if (wolfcore_FIND_QUIETLY)
+    message(STATUS "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+  elseif (wolfcore_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+    # that prevents generation, but continues configuration.
+    message(SEND_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(wolfcore_report_not_found)
+
+if(NOT wolfcore_FOUND)
+  wolfcore_report_not_found("Something went wrong while setting up wolf.")
+else (NOT wolfcore_FOUND)
+  # Set the include directories for wolf core (itself).
+  set(wolfcore_FOUND TRUE)
+
+  # Now we gather all the required dependencies for Wolf
+
+  get_filename_component(WOLFCORE_CURRENT_CONFIG_DIR
+    "${CMAKE_CURRENT_LIST_FILE}" PATH)
+
+  SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH})
+  LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLFCORE_CURRENT_CONFIG_DIR})
+
+  FIND_PACKAGE(Threads REQUIRED)
+  list(APPEND wolfcore_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
+
+  FIND_PACKAGE(Ceres REQUIRED)
+  list(APPEND wolfcore_INCLUDE_DIRS ${CERES_INCLUDE_DIRS})
+  list(APPEND wolfcore_LIBRARIES ${CERES_LIBRARIES})
+
+  # YAML with yaml-cpp
+  FIND_PACKAGE(YamlCpp REQUIRED)
+  list(APPEND wolfcore_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
+  list(APPEND wolfcore_LIBRARIES ${YAMLCPP_LIBRARY})
+
+  #Eigen
+  # FIND_PACKAGE(Eigen3 REQUIRED)
+  # list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+
+  if(NOT Eigen3_FOUND)
+    FIND_PACKAGE(Eigen3 REQUIRED)
+  endif()
+  if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
+    message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
+  endif()
+  list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+endif(NOT wolfcore_FOUND)
+SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 6325f785bd2cbfab046f31b73e419c01adf3d7ff..0fd8d42cc5280016d7473e23cb2624615221fda7 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -10,10 +10,10 @@
 #include <cmath>
 #include <queue>
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 //Wolf includes
 #include "wolf_manager.h"
 #include "core/capture/capture_void.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
@@ -62,12 +62,14 @@ int main(int argc, char** argv)
     SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
 
     // Ceres wrapper
-    ceres::Solver::Options ceres_options;
-    ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
-    ceres_options.max_line_search_step_contraction = 1e-3;
-    ceres_options.max_num_iterations = 1e4;
-    CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options);
-    CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options);
+    SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
+    SolverCeres* ceres_manager_analytic = new SolverCeres(wolf_problem_analytic, ceres_options);
+    ceres_manager_autodiff.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+    ceres_manager_autodiff.getSolverOptions().max_line_search_step_contraction = 1e-3;
+    ceres_manager_autodiff.getSolverOptions().max_num_iterations = 1e4;
+    ceres_manager_analytic.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+    ceres_manager_analytic.getSolverOptions().max_line_search_step_contraction = 1e-3;
+    ceres_manager_analytic.getSolverOptions().max_num_iterations = 1e4;
 
     // load graph from .txt
     offLineFile_.open(file_path_.c_str(), std::ifstream::in);
@@ -272,8 +274,8 @@ int main(int argc, char** argv)
         printf("\nError opening file\n");
 
     // PRIOR
-    FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
-    FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
+    FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
+    FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
     CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
     CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
     first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index f19095d3400f9c08b9633e6486398a0d059bc707..386d803138f33b89bee401aceda047fe2199f104 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -10,10 +10,10 @@
 #include <cmath>
 #include <queue>
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 //Wolf includes
 #include "wolf_manager.h"
 #include "core/capture/capture_void.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
@@ -72,8 +72,8 @@ int main(int argc, char** argv)
     ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
     ceres_options.max_line_search_step_contraction = 1e-3;
     ceres_options.max_num_iterations = 1e4;
-    CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options);
-    CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options);
+    SolverCeres* ceres_manager_full = new SolverCeres(wolf_problem_full, ceres_options);
+    SolverCeres* ceres_manager_prun = new SolverCeres(wolf_problem_prun, ceres_options);
 
     // load graph from .txt
     offLineFile_.open(file_path_.c_str(), std::ifstream::in);
@@ -130,8 +130,8 @@ int main(int argc, char** argv)
                     bNum.clear();
 
                     // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
                     wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
                     wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
                     // store
@@ -301,8 +301,8 @@ int main(int argc, char** argv)
         printf("\nError opening file\n");
 
     // PRIOR
-    FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
-    FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
+    FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
+    FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
     CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
     CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
     first_frame_full->addCapture(initial_covariance_full);
diff --git a/demos/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt
index bf223af61a3358cfc458d8360c1b07e7ab1aff01..81291a479cd3e524ba77b3676982ed0e140d965b 100644
--- a/demos/hello_wolf/CMakeLists.txt
+++ b/demos/hello_wolf/CMakeLists.txt
@@ -22,17 +22,17 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
     
 
 add_library(hellowolf SHARED ${SRCS_HELLOWOLF})
-TARGET_LINK_LIBRARIES(hellowolf ${PROJECT_NAME})
+TARGET_LINK_LIBRARIES(hellowolf ${PLUGIN_NAME})
 
 ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
-TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf)
+TARGET_LINK_LIBRARIES(hello_wolf ${PLUGIN_NAME} hellowolf)
 
 ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
-#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME})
-TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME} hellowolf)
+#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME})
+TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME} hellowolf)
 
 #add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2d.cpp ../src/processor/processor_odom_2d.cpp)
-#TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
+#TARGET_LINK_LIBRARIES(sensor_odom ${PLUGIN_NAME} hellowolf)
 #
 #add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
-#TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME} hellowolf)
+#TARGET_LINK_LIBRARIES(range_bearing ${PLUGIN_NAME} hellowolf)
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index e51f7a33658bf1c3afb9f2d20e8118825886ebf6..2b2d03d43f1a7d315efb0ae07a734ec022f9e4c6 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -14,13 +14,11 @@
 
 
 // wolf core includes
+#include "../../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/common/wolf.h"
 #include "core/sensor/sensor_odom_2d.h"
 #include "core/processor/processor_odom_2d.h"
 #include "core/capture/capture_odom_2d.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// hello wolf local includes
 #include "sensor_range_bearing.h"
 #include "processor_range_bearing.h"
 #include "capture_range_bearing.h"
@@ -106,12 +104,11 @@ int main()
 
     // Wolf problem and solver
     ProblemPtr problem                      = Problem::create("PO", 2);
-    ceres::Solver::Options options;
-    options.max_num_iterations              = 1000; // We depart far from solution, need a lot of iterations
-    CeresManagerPtr ceres                   = std::make_shared<CeresManager>(problem, options);
+    SolverCeresPtr ceres                    = std::make_shared<SolverCeres>(problem);
+    ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
 
     // sensor odometer 2d
-    ParamsSensorOdom2dPtr intrinsics_odo      = std::make_shared<ParamsSensorOdom2d>();
+    ParamsSensorOdom2dPtr intrinsics_odo    = std::make_shared<ParamsSensorOdom2d>();
     SensorBasePtr sensor_odo                = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
 
     // processor odometer 2d
@@ -235,13 +232,12 @@ int main()
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto& kf : problem->getTrajectory()->getFrameList())
-        if (kf->isKeyOrAux())
-        {
-            Eigen::MatrixXd cov;
-            kf->getCovariance(cov);
-            WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
-        }
+    for (auto& kf : *problem->getTrajectory())
+    {
+        Eigen::MatrixXd cov;
+        kf->getCovariance(cov);
+        WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+    }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
         Eigen::MatrixXd cov;
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index fbe33c3afd885a4ccafdb72a10bad23d19cf507a..edeb6b1f04f1f673ad3af9b94493b4e67e46ab9f 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -7,13 +7,12 @@
 
 
 // wolf core includes
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/solver/factory_solver.h"
 #include "core/common/wolf.h"
 #include "core/capture/capture_odom_2d.h"
 #include "core/processor/processor_motion.h"
 #include "core/yaml/parser_yaml.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// hello wolf local includes
 #include "capture_range_bearing.h"
 
 
@@ -112,9 +111,10 @@ int main()
     problem->print(4,0,1,0);
 
     // Solver. Configure a Ceres solver
-    ceres::Solver::Options options;
-    options.max_num_iterations  = 1000; // We depart far from solution, need a lot of iterations
-    CeresManagerPtr ceres       = std::make_shared<CeresManager>(problem, options);
+    // ceres::Solver::Options options;
+    // options.max_num_iterations  = 1000; // We depart far from solution, need a lot of iterations
+    // SolverCeresPtr ceres       = std::make_shared<SolverCeres>(problem, options);
+    SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
 
     // recover sensor pointers and other stuff for later use (access by sensor name)
     SensorBasePtr sensor_odo    = problem->getSensor("sen odom");
@@ -218,13 +218,12 @@ int main()
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto& kf : problem->getTrajectory()->getFrameList())
-        if (kf->isKeyOrAux())
-        {
-            Eigen::MatrixXd cov;
-            kf->getCovariance(cov);
-            WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
-        }
+    for (auto& kf : *problem->getTrajectory())
+    {
+        Eigen::MatrixXd cov;
+        kf->getCovariance(cov);
+        WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+    }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
         Eigen::MatrixXd cov;
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 08e77f76c8d7d2ec70c9b1ac6689e724a67e5c7c..7df38e73367b7b45aff81cdbf5f00099d64aa990 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -46,7 +46,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
     if (!kf)
     {
         // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
-        kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp());
+        kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
         assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
     }
 
diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
index bdcefb3a7141627523a6f2111307fc776abbadbb..f086da9071e4956e3c7ef1ab2d59bff81326f594 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -33,7 +33,7 @@ struct ParamsProcessorRangeBearing : public ParamsProcessorBase
         }
         std::string print() const
         {
-            return "\n" + ParamsProcessorBase::print();
+            return ParamsProcessorBase::print();
         }
 };
 
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 59c217fe0f3f9f5f70973dca8a2ef4b060a6696a..70684f77951f2681d8c2741d3c5c5b09fc503a12 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -31,7 +31,7 @@ struct ParamsSensorRangeBearing : public ParamsSensorBase
     }
     std::string print() const
     {
-        return "" + ParamsSensorBase::print()                                         + "\n"
+        return ParamsSensorBase::print()                                         + "\n"
         + "noise_range_metres_std: "    + std::to_string(noise_range_metres_std)    + "\n"
         + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
     }
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index f2e4f8db31f97ef8059b4d59483a747456e6d19c..99f31be90ac5f4302127c0213c768e41ac9e9b80 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -2,24 +2,31 @@ config:
 
   problem:
   
-    tree_manager:
-      type: "TreeManagerSlidingWindow"
-      n_key_frames: -1
-      fix_first_key_frame: false
-      viral_remove_empty_parent: true
-    frame_structure:      "PO"            # keyframes have position and orientation
     dimension:            2               # space is 2d
+    frame_structure:      "PO"            # keyframes have position and orientation
+  
     prior:
       mode:               "factor"
-      # state:              [0,0,0]
       $state:
         P: [0,0]
         O: [0]
-      # cov:                [[3,3],.01,0,0,0,.01,0,0,0,.01]
       $sigma:
         P: [0.31, 0.31]
         O: [0.31]
       time_tolerance:     0.1
+
+    tree_manager:
+      type: "TreeManagerSlidingWindow"
+      n_frames: -1
+      fix_first_frame: false
+      viral_remove_empty_parent: true
+  
+  solver:
+    max_num_iterations: 100
+    verbose: 0
+    period: 0.2
+    update_immediately: false
+    n_threads: 1
     
   sensors:
     
diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
index 7e345ef211f52897b17267065e28feec0bf4b143..328e7783982f12fe4991fbee9270af3878d1ff30 100644
--- a/demos/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -1,4 +1,4 @@
-type:               "ProcessorOdom2d"
+#type:               "ProcessorOdom2d"
 
 time_tolerance:       0.1
 unmeasured_perturbation_std: 0.001
diff --git a/demos/hello_wolf/yaml/processor_range_bearing.yaml b/demos/hello_wolf/yaml/processor_range_bearing.yaml
index 3c257678c9a73eeb3b415e976b55eeddb3a79edd..49a55d90dbb31b7fb18023eba9e71f7c1c03f633 100644
--- a/demos/hello_wolf/yaml/processor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/processor_range_bearing.yaml
@@ -1,4 +1,4 @@
-type:                 "ProcessorRangeBearing"
+#type:                 "ProcessorRangeBearing"
 
 time_tolerance:       0.1
 
diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 65ac28219471ad65180538e526521d7866391a01..0f724c780c41c708b0b5a63a0e429cbd3319cb1e 100644
--- a/demos/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom2d"              # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+#type: "SensorOdom2d"              # This must match the KEY used in the FactorySensor. Otherwise it is an error.
 
 k_disp_to_disp:   0.1  # m^2   / m
 k_rot_to_rot:     0.1  # rad^2 / rad
diff --git a/demos/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
index d1625337dfc0b0d323d662a727c1ac298117a455..7f81091db16c74fb479684a171256a91dd425f66 100644
--- a/demos/hello_wolf/yaml/sensor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -1,4 +1,4 @@
-type:                       "SensorRangeBearing"
+#type:                       "SensorRangeBearing"
 
 noise_range_metres_std:     0.1
 noise_bearing_degrees_std:  0.5
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 8084a74093ba94602ce19dd413deb12c24cfa047..82aab6219874c3575dc14fdb24af6d2cc0e2d07a 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -35,9 +35,8 @@
 
 //Ceres includes
 #include "glog/logging.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 
-//laser_scan_utils
+#include "../../include/core/ceres_wrapper/solver_ceres.h"
 #include "iri-algorithms/laser_scan_utils/corner_detector.h"
 #include "iri-algorithms/laser_scan_utils/entities.h"
 
@@ -181,7 +180,7 @@ int main(int argc, char *argv[])
     //    ceres_options.minimizer_progress_to_stdout = false;
     //    ceres_options.line_search_direction_type = ceres::LBFGS;
     //    ceres_options.max_num_iterations = 100;
-    CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options);
+    SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options);
     std::ofstream log_file, landmark_file;  //output file
 
     // Own solver
@@ -364,8 +363,8 @@ int main(int argc, char *argv[])
     // Vehicle poses
     std::cout << "Vehicle poses..." << std::endl;
     int i = 0;
-    Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
-    for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
+    Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3);
+    for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++)
     {
         if (complex_angle)
             state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index c10374ae82ae1a8f495e6adc3bea5b7f167b548b..18a09ac3e24f36c3cef6b11c87dfc861e17d14be 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -82,9 +82,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
 
 
         // State blocks
-        const StateStructure& getStructure() const;
-        StateBlockPtr getStateBlock(const std::string& _key) const;
-        StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
+        StateBlockPtr getStateBlock(const char& _key) const;
         StateBlockPtr getSensorP() const;
         StateBlockPtr getSensorO() const;
         StateBlockPtr getSensorIntrinsic() const;
@@ -138,17 +136,17 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
 
 inline StateBlockPtr CaptureBase::getSensorP() const
 {
-    return getStateBlock("P");
+    return getStateBlock('P');
 }
 
 inline StateBlockPtr CaptureBase::getSensorO() const
 {
-    return getStateBlock("O");
+    return getStateBlock('O');
 }
 
 inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
 {
-    return getStateBlock("I");
+    return getStateBlock('I');
 }
 
 inline unsigned int CaptureBase::id() const
@@ -158,7 +156,8 @@ inline unsigned int CaptureBase::id() const
 
 inline FrameBasePtr CaptureBase::getFrame() const
 {
-    return frame_ptr_.lock();
+    if(frame_ptr_.expired()) return nullptr;
+    else return frame_ptr_.lock();
 }
 
 inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
deleted file mode 100644
index dd88e5db6bf3b7ac38a925a64ba66c089dbb09ec..0000000000000000000000000000000000000000
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ /dev/null
@@ -1,139 +0,0 @@
-#ifndef CERES_MANAGER_H_
-#define CERES_MANAGER_H_
-
-//Ceres includes
-#include "ceres/jet.h"
-#include "ceres/ceres.h"
-#include "glog/logging.h"
-
-//wolf includes
-#include "core/solver/solver_manager.h"
-#include "core/utils/params_server.h"
-#include "core/ceres_wrapper/cost_function_wrapper.h"
-#include "core/ceres_wrapper/local_parametrization_wrapper.h"
-#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
-
-namespace ceres {
-typedef std::shared_ptr<CostFunction>  CostFunctionPtr;
-}
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CeresManager);
-
-struct CeresParams : public SolverParams
-{
-    CeresParams() = default;
-
-    CeresParams(std::string _unique_name, CeresParams& params)
-    {
-        //
-    }
-
-    ~CeresParams() override = default;
-};
-/** \brief Ceres manager for WOLF
- *
- */
-
-class CeresManager : public SolverManager
-{
-    protected:
-
-        std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
-        std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
-
-        std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
-
-        ceres::Solver::Options ceres_options_;
-        ceres::Solver::Summary summary_;
-        std::unique_ptr<ceres::Problem> ceres_problem_;
-        std::unique_ptr<ceres::Covariance> covariance_;
-
-    public:
-
-        CeresManager(const ProblemPtr& _wolf_problem,
-                     const ceres::Solver::Options& _ceres_options
-                     = ceres::Solver::Options());
-
-        ~CeresManager() override;
-
-        static SolverManagerPtr create(const ProblemPtr& _wolf_problem, const ParamsServer& _server);
-
-        ceres::Solver::Summary getSummary();
-
-        std::unique_ptr<ceres::Problem>& getCeresProblem();
-
-        void computeCovariances(CovarianceBlocksToBeComputed _blocks
-                                        = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
-
-        void computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
-
-        bool hasConverged() override;
-
-        SizeStd iterations() override;
-
-        double initialCost() override;
-
-        double finalCost() override;
-
-        ceres::Solver::Options& getSolverOptions();
-
-
-        const Eigen::SparseMatrixd computeHessian() const;
-
-    protected:
-
-        bool checkDerived(std::string prefix="") const override;
-
-        std::string solveDerived(const ReportVerbosity report_level) override;
-
-        void addFactorDerived(const FactorBasePtr& fac_ptr) override;
-
-        void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
-
-        void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
-
-        void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
-
-        void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
-
-        void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
-
-        ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
-
-        bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
-
-        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
-};
-
-inline ceres::Solver::Summary CeresManager::getSummary()
-{
-    return summary_;
-}
-
-inline std::unique_ptr<ceres::Problem>& CeresManager::getCeresProblem()
-{
-    return ceres_problem_;
-}
-
-inline ceres::Solver::Options& CeresManager::getSolverOptions()
-{
-    return ceres_options_;
-}
-
-inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
-{
-    return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
-            && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
-}
-
-inline bool CeresManager::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr)
-{
-    return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end()
-            && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h
new file mode 100644
index 0000000000000000000000000000000000000000..1717be546ab820d24e13985227dd19753530440e
--- /dev/null
+++ b/include/core/ceres_wrapper/iteration_update_callback.h
@@ -0,0 +1,60 @@
+/*
+ * iteration_callback.h
+ *
+ *  Created on: Jun 15, 2020
+ *      Author: joanvallve
+ */
+
+#ifndef INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_
+#define INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_
+
+#include "core/problem/problem.h"
+#include "ceres/ceres.h"
+
+namespace wolf {
+
+class IterationUpdateCallback : public ceres::IterationCallback
+{
+    public:
+        explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
+        : problem_(_problem)
+        , min_num_iterations_(min_num_iterations)
+        , initial_cost_(0)
+        , verbose_(verbose)
+        {}
+
+        ~IterationUpdateCallback() {}
+
+        ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
+        {
+            if (summary.iteration == 0)
+                initial_cost_ = summary.cost;
+
+            else if (summary.iteration >= min_num_iterations_ and
+                (problem_->getStateBlockNotificationMapSize() != 0 or
+                 problem_->getFactorNotificationMapSize() != 0))
+            {
+                if (summary.cost >= initial_cost_)
+                {
+                    WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration);
+                    return ceres::SOLVER_ABORT;
+                }
+                else
+                {
+                    WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration);
+                    return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
+                }
+            }
+            return ceres::SOLVER_CONTINUE;
+        }
+
+    private:
+        ProblemPtr problem_;
+        int min_num_iterations_;
+        double initial_cost_;
+        bool verbose_;
+};
+
+}
+
+#endif /* INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_ */
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
new file mode 100644
index 0000000000000000000000000000000000000000..0673207bc47e1873084e3f4d711628a67b322f3c
--- /dev/null
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -0,0 +1,214 @@
+#ifndef CERES_MANAGER_H_
+#define CERES_MANAGER_H_
+
+//Ceres includes
+#include "ceres/jet.h"
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+//wolf includes
+#include "core/solver/solver_manager.h"
+#include "core/utils/params_server.h"
+
+namespace ceres {
+typedef std::shared_ptr<CostFunction>  CostFunctionPtr;
+}
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(SolverCeres);
+WOLF_PTR_TYPEDEFS(LocalParametrizationWrapper);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
+
+struct ParamsCeres : public ParamsSolver
+{
+    bool update_immediately = false;
+    int min_num_iterations = 1;
+    ceres::Solver::Options solver_options;
+    ceres::Problem::Options problem_options;
+    ceres::Covariance::Options covariance_options;
+
+    ParamsCeres() :
+        ParamsSolver()
+    {
+        loadHardcodedValues();
+    }
+
+    ParamsCeres(std::string _unique_name, const ParamsServer& _server) :
+        ParamsSolver(_unique_name, _server)
+    {
+        loadHardcodedValues();
+
+        // stop solver whenever the problem is updated (via ceres::iterationCallback)
+        update_immediately                      = _server.getParam<bool>(prefix + "update_immediately");
+        if (update_immediately)
+            min_num_iterations                  = _server.getParam<int>(prefix + "min_num_iterations");
+
+        // ceres solver options
+        solver_options.max_num_iterations       = _server.getParam<int>(prefix + "max_num_iterations");
+        solver_options.num_threads              = _server.getParam<int>(prefix + "n_threads");
+        covariance_options.num_threads          = _server.getParam<int>(prefix + "n_threads");
+    }
+
+    void loadHardcodedValues()
+    {
+        solver_options = ceres::Solver::Options();
+        solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH;
+        solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG;
+        problem_options = ceres::Problem::Options();
+        covariance_options = ceres::Covariance::Options();
+        problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+        problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
+        problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+
+        #if CERES_VERSION_MINOR >= 13
+            covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+            covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
+        #elif CERES_VERSION_MINOR >= 10
+            covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+        #else
+            covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
+        #endif
+        covariance_options.apply_loss_function = false;
+    }
+
+    ~ParamsCeres() override = default;
+};
+
+class SolverCeres : public SolverManager
+{
+    protected:
+
+        std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
+        std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
+
+        // this map is only for handling automatic destruction of localParametrizationWrapper objects
+        std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
+
+        ceres::Solver::Summary summary_;
+        std::unique_ptr<ceres::Problem> ceres_problem_;
+        std::unique_ptr<ceres::Covariance> covariance_;
+
+        ParamsCeresPtr params_ceres_;
+
+    public:
+
+        SolverCeres(const ProblemPtr& _wolf_problem);
+
+        SolverCeres(const ProblemPtr& _wolf_problem,
+                    const ParamsCeresPtr& _params);
+
+        WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
+
+        ~SolverCeres() override;
+
+        ceres::Solver::Summary getSummary();
+
+        std::unique_ptr<ceres::Problem>& getCeresProblem();
+
+        bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
+                                        = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
+
+        bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
+
+        bool hasConverged() override;
+
+        SizeStd iterations() override;
+
+        double initialCost() override;
+
+        double finalCost() override;
+
+        ceres::Solver::Options& getSolverOptions();
+
+        const Eigen::SparseMatrixd computeHessian() const;
+
+        virtual int numStateBlocksDerived() const override;
+
+        virtual int numFactorsDerived() const override;
+
+    protected:
+
+        bool checkDerived(std::string prefix="") const override;
+
+        std::string solveDerived(const ReportVerbosity report_level) override;
+
+        void addFactorDerived(const FactorBasePtr& fac_ptr) override;
+
+        void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
+
+        void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
+
+        void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
+
+        void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
+
+        void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
+
+        ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
+
+        bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
+
+        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+
+        bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+
+        bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                const LocalParametrizationBasePtr& local_param) override;
+
+        bool hasLocalParametrizationDerived(const StateBlockPtr& st)  const override;
+};
+
+inline ceres::Solver::Summary SolverCeres::getSummary()
+{
+    return summary_;
+}
+
+inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem()
+{
+    return ceres_problem_;
+}
+
+inline ceres::Solver::Options& SolverCeres::getSolverOptions()
+{
+    return params_ceres_->solver_options;
+}
+
+inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
+{
+    return fac_2_residual_idx_.count(fac_ptr) == 1 and
+           fac_2_costfunction_.count(fac_ptr) == 1;
+}
+
+inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
+{
+    if (state_blocks_.count(state_ptr) == 0)
+        return false;
+    return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+}
+
+inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
+{
+    if (state_blocks_.count(st) == 0)
+        return false;
+    return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
+};
+
+inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
+{
+    return state_blocks_local_param_.count(st) == 1;
+};
+
+inline int SolverCeres::numStateBlocksDerived() const
+{
+    return ceres_problem_->NumParameterBlocks();
+}
+
+inline int SolverCeres::numFactorsDerived() const
+{
+    return ceres_problem_->NumResidualBlocks();
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index 80f6516415e28ddce47ab272415604014ffb0a66..b9a4886c290fb9c1dcb4889d55b6d1523c42df78 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -13,10 +13,7 @@ namespace wolf {
     }
 
     virtual ~ParamsBase() = default;
-    std::string print() const
-    {
-        return "";
-    }
+    virtual std::string print() const = 0;
   };
 }
 #endif
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 229e10f95dd8f7506846fe75536a85c6604ae521..97f9a50ff1eb1b0a345461af2fbdce2b81b32491 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -23,6 +23,7 @@
 #include <list>
 #include <map>
 #include <memory> // shared_ptr and weak_ptr
+#include <set>
 
 // System specifics
 #include <sys/stat.h>
@@ -230,10 +231,14 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase);
 // Trajectory
 WOLF_PTR_TYPEDEFS(TrajectoryBase);
 
+
 // - Frame
 WOLF_PTR_TYPEDEFS(FrameBase);
 WOLF_LIST_TYPEDEFS(FrameBase);
 
+class TimeStamp;
+typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+
 // - Capture
 WOLF_PTR_TYPEDEFS(CaptureBase);
 WOLF_LIST_TYPEDEFS(CaptureBase);
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 1d4f01a35e5a1541ec6b147ab3155d268a7f9a9c..c600f8a2433a024f63d2af21fe39105c761e7300 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -4,6 +4,8 @@
 // Forward declarations for node templates
 namespace wolf{
 class FeatureBase;
+class TrajectoryIter;
+class TrajectoryRevIter;
 }
 
 //Wolf includes
diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_odom_2d_analytic.h
index dc1437e4fe441c8c9a6e9406097ac757f56d8c03..c98a521dfdc638d250de0406c616b51469df6390 100644
--- a/include/core/factor/factor_odom_2d_analytic.h
+++ b/include/core/factor/factor_odom_2d_analytic.h
@@ -35,15 +35,6 @@ class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
             return std::string("MOTION");
         }
 
-    public:
-//        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
-//                                    const NodeBasePtr& _correspondant_ptr,
-//                                    const ProcessorBasePtr& _processor_ptr = nullptr)
-//        {
-//            return std::make_shared<FactorOdom2dAnalytic>(_feature_ptr,
-//                                                          std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-//        }
-
 };
 
 } // namespace wolf
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 91a94d274b36d0023a0ba267583f4b67726d8665..c4e423947d5ce6c1c00d2f70f6c510dea38e1608 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -1,4 +1,5 @@
 
+
 #ifndef FRAME_BASE_H_
 #define FRAME_BASE_H_
 
@@ -7,15 +8,6 @@ namespace wolf{
 class TrajectoryBase;
 class CaptureBase;
 class StateBlock;
-
-/** \brief Enumeration of frame types
- */
-typedef enum
-{
-    KEY = 2,          ///< key frame. It plays at optimizations (estimated).
-    AUXILIARY = 1,    ///< auxiliary frame. It plays at optimizations (estimated).
-    NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
-} FrameType;
 }
 
 //Wolf includes
@@ -44,7 +36,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
 
     protected:
         unsigned int frame_id_;
-        FrameType type_;     ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h)
         TimeStamp time_stamp_;     ///< frame time stamp
         
     public:
@@ -57,39 +48,34 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
          * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
          * \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
          **/        
-        FrameBase(const FrameType & _tp,
-                  const TimeStamp& _ts,
+        FrameBase(const TimeStamp& _ts,
                   StateBlockPtr _p_ptr,
                   StateBlockPtr _o_ptr = nullptr,
                   StateBlockPtr _v_ptr = nullptr);
 
-        FrameBase(const FrameType & _tp,
-                  const TimeStamp& _ts,
+        FrameBase(const TimeStamp& _ts,
                   const std::string _frame_structure,
                   const VectorComposite& _state);
 
-        FrameBase(const FrameType & _tp,
-                  const TimeStamp& _ts,
+        FrameBase(const TimeStamp& _ts,
                   const std::string _frame_structure,
                   const std::list<VectorXd>& _vectors);
 
         ~FrameBase() override;
+
+        // Add and remove from WOLF tree ---------------------------------
+        template<typename classType, typename... T>
+        static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+
+        void link(TrajectoryBasePtr);
+        void link(ProblemPtr _prb);
+
         virtual void remove(bool viral_remove_empty_parent=false);
 
         // Frame properties -----------------------------------------------
     public:
         unsigned int id() const;
 
-        // get type
-        bool isKey() const;
-        bool isAux() const;
-        bool isKeyOrAux() const;
-
-        // set type
-        void setNonEstimated();
-        void setKey();
-        void setAux();
-
         // Frame values ------------------------------------------------
     public:
         void        setTimeStamp(const TimeStamp& _ts);
@@ -99,7 +85,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         // State blocks ----------------------------------------------------
     public:
         using HasStateBlocks::addStateBlock;
-        StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
+        StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb);
         StateBlockPtr getV() const;
 
     protected:
@@ -124,15 +110,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
 
         FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
         FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
-
         void getFactorList(FactorBasePtrList& _fac_list) const;
+
         unsigned int getHits() const;
         const FactorBasePtrList& getConstrainedByList() const;
         bool isConstrainedBy(const FactorBasePtr& _factor) const;
-        void link(TrajectoryBasePtr);
-        template<typename classType, typename... T>
-        static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
 
+
+        // Debug and info -------------------------------------------------------
         virtual void printHeader(int depth, //
                                  bool constr_by, //
                                  bool metric, //
@@ -184,20 +169,11 @@ inline unsigned int FrameBase::id() const
     return frame_id_;
 }
 
-inline bool FrameBase::isKey() const
-{
-    return (type_ == KEY);
-}
-
-inline bool FrameBase::isAux() const
-{
-    return (type_ == AUXILIARY);
-}
-
-inline bool FrameBase::isKeyOrAux() const
-{
-    return (type_ == KEY || type_ == AUXILIARY);
-}
+//inline bool FrameBase::isKey() const
+//{
+//    return true;
+////    return (type_ == KEY);
+//}
 
 inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
 {
@@ -211,7 +187,7 @@ inline TimeStamp FrameBase::getTimeStamp() const
 
 inline StateBlockPtr FrameBase::getV() const
 {
-    return getStateBlock("V");
+    return getStateBlock('V');
 }
 
 inline TrajectoryBasePtr FrameBase::getTrajectory() const
@@ -234,13 +210,10 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
     return constrained_by_list_;
 }
 
-inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type,
+inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type,
                                               const StateBlockPtr& _sb)
 {
-    if (isKeyOrAux())
-        HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
-    else
-        HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
+    HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
 
     return _sb;
 }
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 4ac32d2ded2923f212937cfa88a0994fb0dca5b4..ffd8ac8725e49ea921b32140042eb0936cf5d467 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -36,6 +36,12 @@ inline Eigen::Matrix<T, 2, 2> exp(const T theta)
 
 namespace SE2{
 
+template<class T>
+inline Eigen::Matrix<T, 2, 2> skew(const T& t)
+{
+        return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished();
+}
+
 /** \brief Compute  [1]_x * t = (-t.y ; t.x)
  */
 template<class D>
@@ -69,8 +75,8 @@ inline Eigen::Matrix2d V_helper(const T theta)
 inline VectorComposite identity()
 {
     VectorComposite v;
-    v["P"] = Vector2d::Zero();
-    v["O"] = Vector1d::Zero();
+    v['P'] = Vector2d::Zero();
+    v['O'] = Vector1d::Zero();
 
     return v;
 }
@@ -138,12 +144,12 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
 inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
 {
     // [1] eq. 156
-    const auto &p = _tau.at("P");
-    const auto &theta = pi2pi(_tau.at("O")(0));
+    const auto &p = _tau.at('P');
+    const auto &theta = pi2pi(_tau.at('O')(0));
     Matrix2d V = V_helper(theta);
 
-    _delta["P"] = V * p;
-    _delta["O"] = Matrix1d(theta);
+    _delta['P'] = V * p;
+    _delta['O'] = Matrix1d(theta);
 }
 
 inline VectorComposite exp(const VectorComposite& tau)
@@ -158,22 +164,22 @@ inline VectorComposite exp(const VectorComposite& tau)
 inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
 {
     // [1] eq. 156
-    const auto &p = _tau.at("P");
-    const auto &theta = pi2pi(_tau.at("O")(0));
+    const auto &p = _tau.at('P');
+    const auto &theta = pi2pi(_tau.at('O')(0));
     Matrix2d V = V_helper(theta);
 
-    _delta["P"] = V * p;
-    _delta["O"] = Matrix1d(theta);
+    _delta['P'] = V * p;
+    _delta['O'] = Matrix1d(theta);
 
     // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
     //   J_Vp_p = V: see V_helper, eq. 158
     //   J_Vp_theta: see fcn helper
     //   J_theta_theta = 1; eq. 126
     _J_delta_tau.clear();
-    _J_delta_tau.emplace("P", "P", V);
-    _J_delta_tau.emplace("P", "O", J_Vp_theta(p, theta));
-    _J_delta_tau.emplace("O", "P", RowVector2d(0.0, 0.0));
-    _J_delta_tau.emplace("O", "O", Matrix1d(1));
+    _J_delta_tau.emplace('P', 'P', V);
+    _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
+    _J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0));
+    _J_delta_tau.emplace('O', 'O', Matrix1d(1));
 }
 
 template<class D1, class D2, class D3>
@@ -247,18 +253,18 @@ inline void compose(const VectorComposite& _x1,
                     const VectorComposite& _x2,
                     VectorComposite& _c)
 {
-    const auto& p1 = _x1.at("P");
-    const auto& a1 = _x1.at("O")(0); // angle
+    const auto& p1 = _x1.at('P');
+    const auto& a1 = _x1.at('O')(0); // angle
     const auto& R1 = SO2::exp(a1);
 
-    const auto& p2 = _x2.at("P");
-    const auto& a2 = _x2.at("O")(0); // angle
+    const auto& p2 = _x2.at('P');
+    const auto& a2 = _x2.at('O')(0); // angle
 
     // tc = t1 + R1 t2
-    _c["P"] = p1 + R1 * p2;
+    _c['P'] = p1 + R1 * p2;
 
     // Rc = R1 R2 --> theta = theta1 + theta2
-    _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+    _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
 }
 
 inline void compose(const VectorComposite& _x1,
@@ -267,12 +273,12 @@ inline void compose(const VectorComposite& _x1,
              MatrixComposite& _J_c_x1,
              MatrixComposite& _J_c_x2)
 {
-    const auto& p1 = _x1.at("P");
-    const auto& a1 = _x1.at("O")(0); // angle
+    const auto& p1 = _x1.at('P');
+    const auto& a1 = _x1.at('O')(0); // angle
     Matrix2d    R1 = SO2::exp(a1); // need temporary
 
-    const auto& p2 = _x2.at("P");
-    const auto& a2 = _x2.at("O")(0); // angle
+    const auto& p2 = _x2.at('P');
+    const auto& a2 = _x2.at('O')(0); // angle
 
     /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
      *
@@ -302,25 +308,25 @@ inline void compose(const VectorComposite& _x1,
      */
 
     _J_c_x1.clear();
-    _J_c_x1.emplace("P", "P", Matrix2d::Identity());
-    _J_c_x1.emplace("P", "O", MatrixXd(R1 * skewed(p2)) );
-    _J_c_x1.emplace("O", "P", RowVector2d(0,0));
-    _J_c_x1.emplace("O", "O", Matrix1d(1));
+    _J_c_x1.emplace('P', 'P', Matrix2d::Identity());
+    _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) );
+    _J_c_x1.emplace('O', 'P', RowVector2d(0,0));
+    _J_c_x1.emplace('O', 'O', Matrix1d(1));
 
     _J_c_x2.clear();
-    _J_c_x2.emplace("P", "P", R1);
-    _J_c_x2.emplace("P", "O", Vector2d(0,0));
-    _J_c_x2.emplace("O", "P", RowVector2d(0,0));
-    _J_c_x2.emplace("O", "O", Matrix1d(1));
+    _J_c_x2.emplace('P', 'P', R1);
+    _J_c_x2.emplace('P', 'O', Vector2d(0,0));
+    _J_c_x2.emplace('O', 'P', RowVector2d(0,0));
+    _J_c_x2.emplace('O', 'O', Matrix1d(1));
 
 
     // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
 
     // tc = t1 + R1 t2
-    _c["P"] = p1 + R1 * p2;
+    _c['P'] = p1 + R1 * p2;
 
     // Rc = R1 R2 --> theta = theta1 + theta2
-    _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+    _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
 
 }
 
@@ -366,15 +372,15 @@ inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
 
 inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
 {
-    plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+    plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O'));
 }
 
 inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
 {
     VectorComposite res;
 
-    res["P"] = Vector2d();
-    res["O"] = Vector1d();
+    res['P'] = Vector2d();
+    res['O'] = Vector1d();
 
     plus(x, tau, res);
 
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 4b000889c59320e8c7e819725c415dd19308d077..685eefabaccf1d0585ec7de7d73c9d4ba2e1ff8d 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -67,8 +67,8 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
 inline void identity(VectorComposite& v)
 {
     v.clear();
-    v["P"] = Vector3d::Zero();
-    v["O"] = Quaterniond::Identity().coeffs();
+    v['P'] = Vector3d::Zero();
+    v['O'] = Quaterniond::Identity().coeffs();
 }
 
 template<typename T = double>
@@ -207,7 +207,7 @@ inline void compose(const MatrixBase<D1>& d1,
 
 inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c)
 {
-    compose(x1.at("P"), x1.at("O"), x2.at("P"), x2.at("O"), c["P"], c["O"]);
+    compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
 }
 
 inline void compose(const VectorComposite& x1,
@@ -218,20 +218,20 @@ inline void compose(const VectorComposite& x1,
 {
 
     // Some useful temporaries
-    const auto  R1 = q2R(x1.at("O")); //q1.matrix(); // make temporary
-    const auto& R2 = q2R(x2.at("O")); //q2.matrix(); // do not make temporary, only reference
+    const auto  R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary
+    const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference
 
     // Jac wrt first delta
-    J_c_x1.emplace("P", "P", Matrix3d::Identity()    ) ;
-    J_c_x1.emplace("P", "O", - R1 * skew(x2.at("P")) ) ;
-    J_c_x1.emplace("O", "P", Matrix3d::Zero()        ) ;
-    J_c_x1.emplace("O", "O", R2.transpose()          ) ;
+    J_c_x1.emplace('P', 'P', Matrix3d::Identity()    ) ;
+    J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ;
+    J_c_x1.emplace('O', 'P', Matrix3d::Zero()        ) ;
+    J_c_x1.emplace('O', 'O', R2.transpose()          ) ;
 
     // Jac wrt second delta
-    J_c_x2.emplace("P", "P", R1                   );
-    J_c_x2.emplace("P", "O", Matrix3d::Zero()     );
-    J_c_x2.emplace("O", "P", Matrix3d::Zero()     );
-    J_c_x2.emplace("O", "O", Matrix3d::Identity() );
+    J_c_x2.emplace('P', 'P', R1                   );
+    J_c_x2.emplace('P', 'O', Matrix3d::Zero()     );
+    J_c_x2.emplace('O', 'P', Matrix3d::Zero()     );
+    J_c_x2.emplace('O', 'O', Matrix3d::Identity() );
 
     // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
     compose(x1, x2, c);
@@ -325,15 +325,15 @@ inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_i
 
 inline VectorComposite exp(const VectorComposite& tau)
 {
-    const auto& p     = tau.at("P");
-    const auto& theta = tau.at("O");
+    const auto& p     = tau.at('P');
+    const auto& theta = tau.at('O');
 
     Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
 
     VectorComposite  res;
 
-    res["P"] = V * p                 ;
-    res["O"] = exp_q(theta).coeffs() ;
+    res['P'] = V * p                 ;
+    res['O'] = exp_q(theta).coeffs() ;
 
     return res;
 }
@@ -392,15 +392,15 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
 
 inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
 {
-    plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+    plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res.at('P'), res.at('O'));
 }
 
 inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
 {
     VectorComposite res;
 
-    res["P"] = Vector3d();
-    res["O"] = Vector4d();
+    res['P'] = Vector3d::Zero();
+    res['O'] = Vector4d::Zero();
 
     plus(x, tau, res);
 
diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h
index 4721c3def15ec0f5cdf8371387daf989402dcc12..45cbdf3e1366fab06820ca54fad65cfa7e3af5fd 100644
--- a/include/core/math/covariance.h
+++ b/include/core/math/covariance.h
@@ -22,13 +22,15 @@ inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
 template <typename T, int N, int RC>
 inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
 {
-  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
 
   if (eigensolver.info() == Eigen::Success)
   {
     // All eigenvalues must be >= 0:
     return (eigensolver.eigenvalues().array() >= eps).all();
   }
+  else
+      std::cout << "eigen decomposition failed" << std::endl;
 
   return false;
 }
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index f64f629056e3c5c52abc41818f049dc14b876438..5ab5f053fee0c4559a4f8fd473dee43777f54197 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -69,7 +69,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         PriorOptionsPtr prior_options_;
 
     private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
-        Problem(const std::string& _frame_structure); // USE create() below !!
         Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
         void setup();
 
@@ -83,7 +82,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Properties -----------------------------------------
     public:
         SizeEigen getDim() const;
-        StateStructure getFrameStructure() const;
+        const StateStructure& getFrameStructure() const;
 
     protected:
         void appendToStructure(const StateStructure& _structure);
@@ -206,7 +205,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                                           const double &_time_tol);
 
         /** \brief Emplace frame from string frame_structure, dimension and vector
-         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+         * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          * \param _frame_structure String indicating the frame structure.
          * \param _dim variable indicating the dimension of the problem
@@ -217,14 +216,13 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
-                                  const TimeStamp& _time_stamp, //
-                                  const StateStructure& _frame_structure, //
-                                  const SizeEigen _dim, //
-                                  const Eigen::VectorXd& _frame_state);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+                                     const StateStructure& _frame_structure, //
+                                     const SizeEigen _dim, //
+                                     const Eigen::VectorXd& _frame_state);
 
         /** \brief Emplace frame from string frame_structure and state
-         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+         * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          * \param _frame_structure String indicating the frame structure.
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
@@ -236,13 +234,12 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
-                                  const TimeStamp& _time_stamp, //
-                                  const StateStructure& _frame_structure, //
-                                  const VectorComposite& _frame_state);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+                                     const StateStructure& _frame_structure, //
+                                     const VectorComposite& _frame_state);
 
         /** \brief Emplace frame from state
-         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+         * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          * \param _frame_state State; must be part of the problem's frame structure
          *
@@ -254,12 +251,11 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
-                                  const TimeStamp& _time_stamp, //
-                                  const VectorComposite& _frame_state);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+                                     const VectorComposite& _frame_state);
 
         /** \brief Emplace frame from string frame_structure and dimension
-         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+         * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          * \param _frame_structure String indicating the frame structure.
          * \param _dim variable indicating the dimension of the problem
@@ -272,13 +268,12 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
-                                  const TimeStamp& _time_stamp, //
-                                  const StateStructure& _frame_structure, //
-                                  const SizeEigen _dim);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+                                     const StateStructure& _frame_structure, //
+                                     const SizeEigen _dim);
 
         /** \brief Emplace frame from state vector
-         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+         * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          *
@@ -290,12 +285,11 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
-                                  const TimeStamp& _time_stamp, //
-                                  const Eigen::VectorXd& _frame_state);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+                                     const Eigen::VectorXd& _frame_state);
 
         /** \brief Emplace frame, guess all values
-         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+         * \param _frame_key_type Either AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          *
          * - The structure is taken from Problem
@@ -307,15 +301,11 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
-                                  const TimeStamp& _time_stamp);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
 
         // Frame getters
         FrameBasePtr getLastFrame( ) const;
-        FrameBasePtr getLastKeyFrame( ) const;
-        FrameBasePtr getLastKeyOrAuxFrame( ) const;
-        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
-        FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
+        FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
 
         /** \brief Give the permission to a processor to create a new key Frame
          *
@@ -333,22 +323,6 @@ class Problem : public std::enable_shared_from_this<Problem>
                               ProcessorBasePtr _processor_ptr, //
                               const double& _time_tolerance);
 
-        /** \brief Give the permission to a processor to create a new auxiliary Frame
-         *
-         * This should implement a black list of processors that have forbidden auxiliary frame creation.
-         *   - This decision is made at problem level, not at processor configuration level.
-         *   - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
-         */
-        bool permitAuxFrame(ProcessorBasePtr _processor_ptr) const;
-
-        /** \brief New auxiliary frame callback
-         *
-         * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion.
-         */
-        void auxFrameCallback(FrameBasePtr _frame_ptr, //
-                              ProcessorBasePtr _processor_ptr, //
-                              const double& _time_tolerance);
-
         // State getters
         TimeStamp       getTimeStamp    ( ) const;
         VectorComposite getState        ( const StateStructure& _structure = "" ) const;
@@ -380,8 +354,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
         bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
         bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
-        bool getLastKeyFrameCovariance(Eigen::MatrixXd& _covariance) const;
-        bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
+        bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
         bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
 
 
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 09d7bc68dee5511c28ee456bb42467e89168bbb4..dc0487046e5013626ebc02288a00915a7df58b24 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -22,6 +22,7 @@ class IsMotion
 {
     public:
 
+        IsMotion(const StateStructure& _structure);
         virtual ~IsMotion();
 
         // Queries to the processor:
@@ -29,15 +30,33 @@ class IsMotion
         virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
         virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
 
-        std::string getStateStructure(){return state_structure_;};
-        void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
+        VectorComposite getOdometry ( ) const;
+    private:
+        void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
+
+    public:
+        const StateStructure& getStateStructure ( ) { return state_structure_; };
+        void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
         void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
-   
+
     protected:
-        std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+        StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+        VectorComposite odometry_;
 
 };
 
+inline IsMotion::IsMotion(const StateStructure& _structure) :
+        state_structure_(_structure)
+{
+    //
+}
+
+
+inline wolf::VectorComposite IsMotion::getOdometry ( ) const
+{
+    return odometry_;
+}
+
 }
 
 /////  IMPLEMENTATION ///////
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index dd4c37b277a514f40051ee96de70ab04cfe43c62..c8945c8036ff54dc2e0fe2243b3ec0919c66eba4 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -18,6 +18,7 @@ class SensorBase;
 // std
 #include <memory>
 #include <map>
+#include <chrono>
 
 namespace wolf {
 
@@ -96,8 +97,17 @@ public:
     * respecting a defined time tolerances
     */
     T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    
+    /**\brief Select a Pack iterator from the buffer
+    *
+    *  Select from the buffer the iterator pointing to the closest pack (w.r.t. time stamp),
+    * respecting a defined time tolerances
+    */
+    Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
     T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    
+    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
     T selectFirst();
 
@@ -117,7 +127,7 @@ public:
     *
     * elements are ordered from most recent to oldest
     */
-    const std::map<TimeStamp,T>& getContainer();
+    std::map<TimeStamp,T>& getContainer();
 
     /**\brief Remove all packs in the buffer with a time stamp older than the specified
     *
@@ -217,7 +227,6 @@ struct ParamsProcessorBase : public ParamsBase
     {
         time_tolerance      = _server.getParam<double>(prefix + _unique_name + "/time_tolerance");
         voting_active       = _server.getParam<bool>(prefix + _unique_name   + "/keyframe_vote/voting_active");
-        voting_aux_active   = _server.getParam<bool>(prefix + _unique_name   + "/keyframe_vote/voting_aux_active");
         apply_loss_function = _server.getParam<bool>(prefix + _unique_name   + "/apply_loss_function");
     }
 
@@ -229,14 +238,11 @@ struct ParamsProcessorBase : public ParamsBase
      */
     double time_tolerance;
     bool voting_active;         ///< Whether this processor is allowed to vote for a Key Frame or not
-    bool voting_aux_active;     ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
     bool apply_loss_function;   ///< Whether this processor emplaces factors with loss function or not
 
-    std::string print() const
+    std::string print() const override
     {
-        return ParamsBase::print() + "\n"
-                + "voting_active: "         + std::to_string(voting_active)         + "\n"
-                + "voting_aux_active: "     + std::to_string(voting_aux_active)     + "\n"
+        return    "voting_active: "         + std::to_string(voting_active)         + "\n"
                 + "time_tolerance: "        + std::to_string(time_tolerance)        + "\n"
                 + "apply_loss_function: "   + std::to_string(apply_loss_function)   + "\n";
     }
@@ -265,6 +271,21 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         unsigned int id() const;
 
+        // PROFILING
+        unsigned int n_capture_callback_;
+        unsigned int n_kf_callback_;
+        std::chrono::microseconds acc_duration_capture_;
+        std::chrono::microseconds acc_duration_kf_;
+        std::chrono::microseconds max_duration_capture_;
+        std::chrono::microseconds max_duration_kf_;
+        std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
+        std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
+        void startCaptureProfiling();
+        void stopCaptureProfiling();
+        void startKFProfiling();
+        void stopKFProfiling();
+        void printProfiling(std::ostream& stream = std::cout) const;
+
     protected:
         /** \brief process an incoming capture
          *
@@ -313,19 +334,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
          */
         virtual bool voteForKeyFrame() const = 0;
 
-        /** \brief Vote for Auxiliary Frame generation
-         *
-         * If a Auxiliary Frame criterion is validated, this function returns true,
-         * meaning that it wants to create a Auxiliary Frame at the \b last Capture.
-         *
-         * WARNING! This function only votes! It does not create Auxiliary Frames!
-         */
-        virtual bool voteForAuxFrame() const {return false;};
-
         virtual bool permittedKeyFrame() final;
 
-        virtual bool permittedAuxFrame() final;
-
         void setProblem(ProblemPtr) override;
 
     public:
@@ -353,16 +363,15 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         bool isVotingActive() const;
 
-        bool isVotingAuxActive() const;
-
         void setVotingActive(bool _voting_active = true);
 
         int getDim() const;
 
+        double getTimeTolerance() const;
+
         void link(SensorBasePtr);
         template<typename classType, typename... T>
         static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
-        void setVotingAuxActive(bool _voting_active = true);
 
         virtual void printHeader(int depth, //
                                  bool constr_by, //
@@ -382,24 +391,40 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
         bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
 };
 
-inline bool ProcessorBase::isVotingActive() const
+inline void ProcessorBase::startCaptureProfiling()
 {
-    return params_->voting_active;
+    start_capture_ = std::chrono::high_resolution_clock::now();
 }
 
-inline bool ProcessorBase::isVotingAuxActive() const
+inline void ProcessorBase::stopCaptureProfiling()
 {
-    return params_->voting_aux_active;
+    auto duration_capture = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_capture_);
+
+    max_duration_capture_ = std::max(max_duration_capture_, duration_capture);
+    acc_duration_capture_ += duration_capture;
 }
 
-inline void ProcessorBase::setVotingActive(bool _voting_active)
+inline void ProcessorBase::startKFProfiling()
 {
-    params_->voting_active = _voting_active;
+    start_kf_ = std::chrono::high_resolution_clock::now();
 }
 
-inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
+inline void ProcessorBase::stopKFProfiling()
+{
+    auto duration_kf = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_kf_);
+
+    max_duration_kf_ = std::max(max_duration_kf_, duration_kf);
+    acc_duration_kf_ += duration_kf;
+}
+
+inline bool ProcessorBase::isVotingActive() const
 {
-    params_->voting_aux_active = _voting_active;
+    return params_->voting_active;
+}
+
+inline void ProcessorBase::setVotingActive(bool _voting_active)
+{
+    params_->voting_active = _voting_active;
 }
 
 inline bool ProcessorBase::isMotion() const
@@ -426,6 +451,10 @@ inline int ProcessorBase::getDim() const
 {
     return dim_;
 }
+inline double ProcessorBase::getTimeTolerance() const
+{
+    return params_->time_tolerance;
+}
 
 template<typename classType, typename... T>
 std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
@@ -439,11 +468,8 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&...
 /////////////////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
 {
-    if (container_.empty())
-        return nullptr;
-
     Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
 
     bool prev_exists = (post != container_.begin());
@@ -458,22 +484,39 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
         bool prev_ok = simpleCheckTimeTolerance(prev->first, _time_stamp, _time_tolerance);
 
         if (prev_ok && !post_ok)
-            return prev->second;
+            return prev;
 
         else if (!prev_ok && post_ok)
-            return post->second;
+            return post;
 
         else if (prev_ok && post_ok)
         {
             if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
-                return post->second;
+                return post;
             else
-                return prev->second;
+                return prev;
         }
     }
     else if (post_ok)
-        return post->second;
+        return post;
+
+    return container_.end();
+}
+
+template <typename T>
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+{
+    if (container_.empty())
+        return nullptr;
 
+    Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
+
+    // end is returned from selectIterator if an element of the buffer complying with the time stamp
+    // and time tolerance has not been found
+    if (it != container_.end()){
+        return it->second;
+    }
+    
     return nullptr;
 }
 
@@ -495,7 +538,27 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
 
     // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
     return nullptr;
+}
+
 
+template <typename T>
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
+{
+    // There is no element
+    if (container_.empty())
+         return nullptr;
+
+    // Checking on rbegin() since packs are ordered in time
+    // Return last pack if is older than time stamp
+    if (container_.rbegin()->first > _time_stamp)
+         return container_.rbegin()->second;
+
+    // Return last pack if despite being newer, it is within the time tolerance
+    if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
+        return container_.rbegin()->second;
+
+    // otherwise return nullptr (no pack after the provided ts or within the tolerance was found)
+    return nullptr;
 }
 
 template <typename T>
@@ -527,7 +590,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
 }
 
 template <typename T>
-const std::map<TimeStamp,T>& Buffer<T>::getContainer()
+std::map<TimeStamp,T>& Buffer<T>::getContainer()
 {
     return container_;
 }
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index e50b2f55764cbc2a5570a4192ed8c15f560804fe..7203ff3fd48833c5be92de7445dc174193e6a3e1 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -22,9 +22,9 @@ struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d
             ParamsProcessorOdom2d(_unique_name, _server)
         {
         }
-        std::string print() const
+        std::string print() const override
         {
-            return "\n" + ParamsProcessorOdom2d::print();
+            return ParamsProcessorOdom2d::print();
         }
 };
 
@@ -60,7 +60,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
         FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
         FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
                                             CaptureBasePtr _capture_origin) override;
-        VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+        VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
         void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
 
     protected:
@@ -71,12 +71,15 @@ class ProcessorDiffDrive : public ProcessorOdom2d
 
 inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
 {
-    return _capture->getStateBlock("I")->getState();
+    if (_capture)
+        return _capture->getStateBlock('I')->getState();
+    else
+        return getSensor()->getStateBlockDynamic('I')->getState();
 }
 
 inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
 {
-    _capture->getStateBlock("I")->setState(_calibration);
+    _capture->getStateBlock('I')->setState(_calibration);
 }
 
 
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 6d2b81e9ed4425a693c7f669779d1f51a0118845..75053ab0f35d1c338092c723573d5cfbf85755b6 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -41,9 +41,9 @@ struct ParamsProcessorMotion : public ParamsProcessorBase
           angle_turned    = _server.getParam<double>(prefix + _unique_name       + "/keyframe_vote/angle_turned");
           unmeasured_perturbation_std = _server.getParam<double>(prefix + _unique_name + "/unmeasured_perturbation_std");
         }
-        std::string print() const
+        std::string print() const override
         {
-          return "\n" + ParamsProcessorBase::print() + "\n"
+          return ParamsProcessorBase::print() + "\n"
             + "max_time_span: "     + std::to_string(max_time_span)     + "\n"
             + "max_buff_length: "   + std::to_string(max_buff_length)   + "\n"
             + "dist_traveled: "     + std::to_string(dist_traveled)     + "\n"
@@ -247,11 +247,11 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
 
         double updateDt();
         void integrateOneStep();
-        void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
+        void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
         void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
                          TimeStamp ts_split,
                          const FrameBasePtr& keyframe_target,
-                         const wolf::CaptureMotionPtr& capture_target);
+                         const wolf::CaptureMotionPtr& capture_target) const;
 
         /** Pre-process incoming Capture
          *
@@ -281,7 +281,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
         PackKeyFramePtr computeProcessingStep();
 
         // These are the pure virtual functions doing the mathematics
-    protected:
+    public:
 
         /** \brief convert raw CaptureMotion data to the delta-state format
          *
@@ -416,13 +416,26 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
         virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
                                              const Eigen::VectorXd& delta_step) const = 0;
 
+        /** \brief merge two consecutive capture motions into the second one
+         * Merge two consecutive capture motions into the second one.
+         * The first capture motion is not removed.
+         * If the second capture has feature and factor emplaced, they are replaced by a new ones.
+         * @param cap_prev : the first capture motion to be merged (input)
+         * @param cap_target : the second capture motion (modified)
+         */
+        void mergeCaptures(CaptureMotionConstPtr cap_prev,
+                           CaptureMotionPtr cap_target);
+
+    protected:
         /** \brief emplace a CaptureMotion
-         * \param _ts time stamp
+         * \param _frame_own frame owning the Capture to create
          * \param _sensor Sensor that produced the Capture
+         * \param _ts time stamp
          * \param _data a vector of motion data
-         * \param _sata_cov covariances matrix of the motion data data
-         * \param _frame_own frame owning the Capture to create
-         * \param _frame_origin frame acting as the origin of motions for the MorionBuffer of the created MotionCapture
+         * \param _data_cov covariances matrix of the motion data data
+         * \param _calib calibration vector
+         * \param _calib_preint calibration vector used during pre-integration
+         * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
          */
         virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
                                                 const SensorBasePtr& _sensor,
@@ -444,14 +457,15 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          */
         virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
 
-        virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0;
         virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
 
         Motion motionZero(const TimeStamp& _ts) const;
 
+    public:
+
+        virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
         bool hasCalibration() const {return calib_size_ > 0;}
 
-    public:
         //getters
         CaptureMotionPtr getOrigin() const;
         CaptureMotionPtr getLast() const;
@@ -484,21 +498,24 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
         CaptureMotionPtr origin_ptr_;
         CaptureMotionPtr last_ptr_;
         CaptureMotionPtr incoming_ptr_;
-        
+
+        FrameBasePtr last_frame_ptr_;
+
     protected:
         // helpers to avoid allocation
-        double dt_;                             ///< Time step
-        Eigen::VectorXd x_;                     ///< current state
-        Eigen::VectorXd delta_;                 ///< current delta
-        Eigen::MatrixXd delta_cov_;             ///< current delta covariance
-        Eigen::VectorXd delta_integrated_;      ///< integrated delta
-        Eigen::MatrixXd delta_integrated_cov_;  ///< integrated delta covariance
-        Eigen::VectorXd calib_preint_;          ///< calibration vector used during pre-integration
-        Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
-        Eigen::MatrixXd jacobian_delta_;        ///< jacobian of delta composition w.r.t current delta
-        Eigen::MatrixXd jacobian_calib_;        ///< jacobian of delta preintegration wrt calibration params
-        Eigen::MatrixXd jacobian_delta_calib_;  ///< jacobian of delta wrt calib params
-        Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+        mutable double dt_;                             ///< Time step
+        mutable Eigen::VectorXd x_;                     ///< current state
+        mutable Eigen::VectorXd delta_;                 ///< current delta
+        mutable Eigen::MatrixXd delta_cov_;             ///< current delta covariance
+        mutable Eigen::VectorXd delta_integrated_;      ///< integrated delta
+        mutable Eigen::MatrixXd delta_integrated_cov_;  ///< integrated delta covariance
+        mutable Eigen::VectorXd calib_preint_;          ///< calibration vector used during pre-integration
+        mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+        mutable Eigen::MatrixXd jacobian_delta_;        ///< jacobian of delta composition w.r.t current delta
+        mutable Eigen::MatrixXd jacobian_calib_;        ///< jacobian of delta preintegration wrt calibration params
+        mutable Eigen::MatrixXd jacobian_delta_calib_;  ///< jacobian of delta wrt calib params
+
+        Eigen::MatrixXd unmeasured_perturbation_cov_;   ///< Covariance of unmeasured DoF to avoid singularity
 };
 
 }
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index de3aebb4a811e954a27f4ac24c22dd277d9142fd..2ad5b846b59e3c40d98351a0f8b30f75b3f71635 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -31,9 +31,9 @@ struct ParamsProcessorOdom2d : public ParamsProcessorMotion
             cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det");
         }
 
-        std::string print() const
+        std::string print() const override
         {
-            return "\n" + ParamsProcessorMotion::print()    + "\n"
+            return ParamsProcessorMotion::print()    + "\n"
             + "cov_det: "   + std::to_string(cov_det)       + "\n";
         }
 };
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 0ff94590e542afe465676b7332d23ad5064ee56e..676cb6cd05d51240cad7a5008ca301ccb5af67e3 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -27,9 +27,9 @@ struct ParamsProcessorOdom3d : public ParamsProcessorMotion
         {
             //
         }
-        std::string print() const
+        std::string print() const override
         {
-            return "\n" + ParamsProcessorMotion::print();
+            return ParamsProcessorMotion::print();
         }
 };
 
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 0f4c3180003140544f251113583e0184e630d9b8..4d0970d4b4002fa78e1c5566cdeb0165e08ca1f4 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -27,7 +27,7 @@ struct ParamsProcessorTracker : public ParamsProcessorBase
         min_features_for_keyframe   = _server.getParam<unsigned int>(prefix + _unique_name   + "/min_features_for_keyframe");
         max_new_features            = _server.getParam<int>(prefix + _unique_name            + "/max_new_features");
     }
-    std::string print() const
+    std::string print() const override
     {
         return ParamsProcessorBase::print()                                                 + "\n"
                 + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
@@ -101,6 +101,7 @@ class ProcessorTracker : public ProcessorBase
         CaptureBasePtr origin_ptr_;             ///< Pointer to the origin of the tracker.
         CaptureBasePtr last_ptr_;               ///< Pointer to the last tracked capture.
         CaptureBasePtr incoming_ptr_;           ///< Pointer to the incoming capture being processed.
+        FrameBasePtr last_frame_ptr_;
         FeatureBasePtrList new_features_last_;     ///< List of new features in \b last for landmark initialization and new key-frame creation.
         FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
         FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index d59daef943bffdc01e835453963f4dac8c7ad7df..f4cae62c31f2c8389107388bdebedbec72a68b5b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -76,7 +76,7 @@ struct ParamsSensorBase: public ParamsBase
     std::string prefix = "sensor/";
     ~ParamsSensorBase() override = default;
     using ParamsBase::ParamsBase;
-    std::string print() const
+    std::string print() const override
     {
         return "";
     }
@@ -90,20 +90,21 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
     private:
         HardwareBaseWPtr hardware_ptr_;
         ProcessorBasePtrList processor_list_;
-        SizeEigen calib_size_;
 
         static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
 
         unsigned int sensor_id_;   // sensor ID
 
-        std::map<std::string, bool> state_block_dynamic_; // mark dynamic state blocks
+        std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
 
-        std::map<std::string, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+        std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
 
     protected:
         Eigen::VectorXd noise_std_; // std of sensor noise
         Eigen::MatrixXd noise_cov_; // cov matrix of noise
 
+        CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+
         void setProblem(ProblemPtr _problem) override final;
 
     public:
@@ -166,29 +167,27 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
     public:
         const ProcessorBasePtrList& getProcessorList() const;
 
-        CaptureBasePtr lastCapture(void) const;
-        CaptureBasePtr lastKeyCapture(void) const;
-        CaptureBasePtr lastCapture(const TimeStamp& _ts) const;
+        CaptureBasePtr getLastCapture() const;
+        void setLastCapture(CaptureBasePtr);
+        void updateLastCapture();
+        CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const;
 
         bool process(const CaptureBasePtr capture_ptr);
 
         // State blocks
         using HasStateBlocks::addStateBlock;
-        StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
-        StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
-        StateBlockPtr getStateBlockDynamic(const std::string& _key) const;
-        StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
-        StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); }
-        StateBlockPtr getStateBlockDynamic(const char _key, const TimeStamp& _ts) const { return getStateBlockDynamic(std::string(1,_key), _ts); }
+        StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
+        StateBlockPtr getStateBlockDynamic(const char& _key) const;
+        StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
 
         // Declare a state block as dynamic or static (with _dymanic = false)
-        void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
+        void setStateBlockDynamic(const char& _key, bool _dynamic = true);
 
-        bool isStateBlockDynamic(const std::string& _key) const;
-        bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
-        bool isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const;
-        bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const;
-        bool isStateBlockInCapture(const std::string& _key) const;
+        bool isStateBlockDynamic(const char& _key) const;
+        bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
+        bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const;
+        bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const;
+        bool isStateBlockInCapture(const char& _key) const;
 
         StateBlockPtr getP(const TimeStamp _ts) const;
         StateBlockPtr getO(const TimeStamp _ts) const;
@@ -218,7 +217,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          * \param _size state segment size (-1: whole state) (not used in quaternions)
          *
          **/
-        void addPriorParameter(const std::string& _key,
+        void addPriorParameter(const char& _key,
                                const Eigen::VectorXd& _x,
                                const Eigen::MatrixXd& _cov,
                                unsigned int _start_idx = 0,
@@ -234,9 +233,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
                                 unsigned int _start_idx = 0,
                                 int _size = -1);
 
-        SizeEigen getCalibSize() const;
-        Eigen::VectorXd getCalibration() const;
-
         void setNoiseStd(const Eigen::VectorXd & _noise_std);
         void setNoiseCov(const Eigen::MatrixXd & _noise_std);
         Eigen::VectorXd getNoiseStd() const;
@@ -261,12 +257,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         void link(HardwareBasePtr);
         template<typename classType, typename... T>
         static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
-
-    protected:
-        SizeEigen computeCalibSize() const;
-
-    private:
-        void updateCalibSize();
 };
 
 }
@@ -296,7 +286,12 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
     return processor_list_;
 }
 
-inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
+inline CaptureBasePtr SensorBase::getLastCapture() const
+{
+    return last_capture_;
+}
+
+inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
 {
     assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
     HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
@@ -304,13 +299,13 @@ inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const St
     return _sb_ptr;
 }
 
-inline void SensorBase::setStateBlockDynamic(const std::string& _key, bool _dynamic)
+inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic)
 {
     assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
     state_block_dynamic_[_key] = _dynamic;
 }
 
-inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const
+inline bool SensorBase::isStateBlockDynamic(const char& _key) const
 {
     assert(state_block_dynamic_.count(_key) != 0);
     return state_block_dynamic_.at(_key);
@@ -338,27 +333,17 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
 
 inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
 {
-    addPriorParameter("P", _x, _cov, _start_idx, _size);
+    addPriorParameter('P', _x, _cov, _start_idx, _size);
 }
 
 inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov)
 {
-    addPriorParameter("O", _x, _cov);
+    addPriorParameter('O', _x, _cov);
 }
 
 inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
 {
-    addPriorParameter("I", _x, _cov);
-}
-
-inline SizeEigen SensorBase::getCalibSize() const
-{
-    return calib_size_;
-}
-
-inline void SensorBase::updateCalibSize()
-{
-    calib_size_ = computeCalibSize();
+    addPriorParameter('I', _x, _cov);
 }
 
 } // namespace wolf
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 8238139c3a2bf85238a9d4bacc7ac02994e987dd..212a955b19c1a76aecebaa90f7cc39be73f38a7a 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -37,9 +37,9 @@ struct ParamsSensorDiffDrive : public ParamsSensorBase
         prior_cov_diag             = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag");
         ticks_cov_factor           = _server.getParam<double>(prefix + _unique_name + "/ticks_cov_factor");
     }
-    std::string print() const
+    std::string print() const override
     {
-        return "\n" + ParamsSensorBase::print()                                           + "\n"
+        return ParamsSensorBase::print()                                         + "\n"
         + "radius_left: "                   + std::to_string(radius_left)               + "\n"
         + "radius_right: "                  + std::to_string(radius_right)              + "\n"
         + "wheel_separation: "              + std::to_string(wheel_separation)          + "\n"
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index b2a989a6fe2ef55092429d38b0a75698b1627be0..5f2fc1a717b7c58f985ede8845ea2de01dc17fcb 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -25,9 +25,9 @@ struct ParamsSensorOdom2d : public ParamsSensorBase
         k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
         k_rot_to_rot   = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
     }
-    std::string print() const
+    std::string print() const override
     {
-        return "\n" + ParamsSensorBase::print()                               + "\n"
+        return ParamsSensorBase::print()                               + "\n"
                 + "k_disp_to_disp: "    + std::to_string(k_disp_to_disp)    + "\n"
                 + "k_rot_to_rot: "      + std::to_string(k_rot_to_rot)      + "\n";
     }
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index 37ec082f59543e3ad19cc2bea9370abee1e03c23..ab0e8b46872c21015b02ffcb9d521dbd77e2a606 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -36,9 +36,9 @@ struct ParamsSensorOdom3d : public ParamsSensorBase
         min_disp_var   = _server.getParam<double>(prefix + _unique_name + "/min_disp_var");
         min_rot_var    = _server.getParam<double>(prefix + _unique_name + "/min_rot_var");
     }
-    std::string print() const
+    std::string print() const override
     {
-      return "\n" + ParamsSensorBase::print()                      + "\n"
+      return ParamsSensorBase::print()                      + "\n"
         + "k_disp_to_disp: "    + std::to_string(k_disp_to_disp) + "\n"
         + "k_disp_to_rot: "     + std::to_string(k_disp_to_rot)  + "\n"
         + "k_rot_to_rot: "      + std::to_string(k_rot_to_rot)   + "\n"
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 9b2a665facbd1cf2590a81cb311075789b33f575..fb38a8be279f9ed6572c729fdebe1551bb99f4ae 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -10,6 +10,7 @@
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(SolverManager)
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
 
 /*
  * Macro for defining Autoconf solver creator for WOLF's high level API.
@@ -20,137 +21,205 @@ WOLF_PTR_TYPEDEFS(SolverManager)
  * In order to use this macro, the derived solver manager class, SolverManagerClass,
  * must have a constructor available with the API:
  *
- *   SolverManagerClass(const SolverParamsClassPtr _params);
+ *   SolverManagerClass(const ProblemPtr& wolf_problem,
+ *                      const ParamsSolverClassPtr _params);
  */
-#define WOLF_SOLVER_CREATE(SolverClass, SolverParamClass)                                      \
-static SolverManagerPtr create(const std::string& _unique_name,                                \
-                               const ParamsServer& _server)                                    \
-{                                                                                              \
-    auto params = std::make_shared<SolverParamClass>(_unique_name, _server);                   \
-                                                                                               \
-    auto solver = std::make_shared<SolverClass>(params);                                       \
-                                                                                               \
-    solver->setName(_unique_name);                                                             \
-                                                                                               \
-    return solver;                                                                             \
-}                                                                                              \
-static SolverManagerPtr create(const std::string& _unique_name, const SolverParamsPtr _params) \
-{                                                                                              \
-    auto params = std::static_pointer_cast<SolverParamClass>(_params);                         \
-                                                                                               \
-    auto solver = std::make_shared<SolverClass>(params);                                       \
-                                                                                               \
-    solver->setName(_unique_name);                                                             \
-                                                                                               \
-    return solver;                                                                             \
-}                                                                                              \
-
-struct SolverParams: public ParamsBase
-{
-  SolverParams() = default;
-  SolverParams(std::string _unique_name, const ParamsServer& _server):
-    ParamsBase(_unique_name, _server)
-  {
-    //
-  }
-
-    ~SolverParams() override = default;
-};
+#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass)                      \
+static SolverManagerPtr create(const ProblemPtr& _problem,                      \
+                               const ParamsServer& _server)                     \
+{                                                                               \
+    auto params = std::make_shared<ParamsSolverClass>(#SolverClass, _server);   \
+    return std::make_shared<SolverClass>(_problem, params);                     \
+}                                                                               \
+static SolverManagerPtr create(const ProblemPtr& _problem,                      \
+                               const ParamsSolverPtr _params)                   \
+{                                                                               \
+    auto params = std::static_pointer_cast<ParamsSolverClass>(_params);         \
+    return std::make_shared<SolverClass>(_problem, params);                     \
+}                                                                               \
+
+        struct ParamsSolver;
+
 /**
  * \brief Solver manager for WOLF
  */
 class SolverManager
 {
-public:
-
-  /** \brief Enumeration of covariance blocks to be computed
-   *
-   * Enumeration of covariance blocks to be computed
-   *
-   */
-  enum class CovarianceBlocksToBeComputed : std::size_t
-  {
-    ALL, ///< All blocks and all cross-covariances
-    ALL_MARGINALS, ///< All marginals
-    ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-  };
-
-  /**
-   * \brief Enumeration for the verbosity of the solver report.
-   */
-  enum class ReportVerbosity : std::size_t
-  {
-    QUIET = 0,
-    BRIEF,
-    FULL
-  };
-
-protected:
-
-  ProblemPtr wolf_problem_;
-
-public:
-
-  SolverManager(const ProblemPtr& wolf_problem);
+    public:
 
-  virtual ~SolverManager();
+        /** \brief Enumeration of covariance blocks to be computed
+         *
+         * Enumeration of covariance blocks to be computed
+         *
+         */
+        enum class CovarianceBlocksToBeComputed : std::size_t
+        {
+            ALL, ///< All blocks and all cross-covariances
+            ALL_MARGINALS, ///< All marginals
+            ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+            GAUSS
+        };
 
-  std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
+        /**
+         * \brief Enumeration for the verbosity of the solver report.
+         */
+        enum class ReportVerbosity : std::size_t
+        {
+            QUIET = 0,
+            BRIEF,
+            FULL
+        };
 
-  virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+        // PROFILING
+        unsigned int n_solve_, n_cov_;
+        std::chrono::microseconds acc_duration_total_;
+        std::chrono::microseconds acc_duration_solver_;
+        std::chrono::microseconds acc_duration_update_;
+        std::chrono::microseconds acc_duration_state_;
+        std::chrono::microseconds acc_duration_cov_;
 
-  virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
+        std::chrono::microseconds max_duration_total_;
+        std::chrono::microseconds max_duration_solver_;
+        std::chrono::microseconds max_duration_update_;
+        std::chrono::microseconds max_duration_state_;
+        std::chrono::microseconds max_duration_cov_;
 
-  virtual bool hasConverged() = 0;
+        void printProfiling(std::ostream& stream = std::cout) const;
 
-  virtual SizeStd iterations() = 0;
+    protected:
 
-  virtual double initialCost() = 0;
+        ProblemPtr wolf_problem_;
+        ParamsSolverPtr params_;
+
+    public:
+        /**
+         * \brief Constructor with default params_
+         */
+        SolverManager(const ProblemPtr& _problem);
+        /**
+         * \brief Constructor with given params_
+         */
+        SolverManager(const ProblemPtr& _problem,
+                      const ParamsSolverPtr& _params);
+
+        virtual ~SolverManager();
+
+        /**
+         * \brief Solves with the verbosity defined in params_
+         */
+        std::string solve();
+        /**
+         * \brief Solves with a given verbosity
+         */
+        std::string solve(const ReportVerbosity report_level);
+
+        virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
+
+        virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
+
+        virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
+
+        virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
+
+        virtual bool hasConverged() = 0;
+
+        virtual SizeStd iterations() = 0;
+
+        virtual double initialCost() = 0;
+
+        virtual double finalCost() = 0;
+
+        /**
+         * \brief Updates solver's problem according to the wolf_problem
+         */
+        virtual void update();
+
+        ProblemPtr getProblem();
+
+        double getPeriod() const;
+
+        ReportVerbosity getVerbosity() const;
+
+        virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
+
+        virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
+
+        virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+        virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+        virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+                                                 const LocalParametrizationBasePtr& local_param) final;
+
+        virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+        virtual int numFactors() const final;
+        virtual int numStateBlocks() const final;
+        virtual int numStateBlocksFloating() const final;
+
+        virtual int numFactorsDerived() const = 0;
+        virtual int numStateBlocksDerived() const = 0;
+
+        virtual bool check(std::string prefix="") const final;
+
+    protected:
+
+        std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
+        std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
+        std::set<FactorBasePtr> factors_;
+        std::set<StateBlockPtr> floating_state_blocks_;
 
-  virtual double finalCost() = 0;
+        virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+        const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
+        double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
 
-  virtual void update();
+    private:
+        // SolverManager functions
+        virtual void addFactor(const FactorBasePtr& fac_ptr) final;
+        virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
+        virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
+        virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
+        virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
+        virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
+        virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
 
-  ProblemPtr getProblem();
-
-  virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
-
-  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
-
-  bool check(std::string prefix="") const;
-
-protected:
-
-    std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
-    std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
-    std::set<FactorBasePtr> factors_;
-
-    virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
-    const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
-    virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
-
-private:
-    // SolverManager functions
-    void addFactor(const FactorBasePtr& fac_ptr);
-    void removeFactor(const FactorBasePtr& fac_ptr);
-    void addStateBlock(const StateBlockPtr& state_ptr);
-    void removeStateBlock(const StateBlockPtr& state_ptr);
-    void updateStateBlockState(const StateBlockPtr& state_ptr);
-    void updateStateBlockStatus(const StateBlockPtr& state_ptr);
-    void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr);
+    protected:
+        // Derived virtual functions
+        virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
+        virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+        virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+        virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+        virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+        virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
+        virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
+        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
+        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+        virtual bool checkDerived(std::string prefix="") const = 0;
+        virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+        virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                        const LocalParametrizationBasePtr& local_param) = 0;
+        virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
+};
 
-protected:
-    // Derived virtual functions
-    virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
-    virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
-    virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
-    virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
-    virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
-    virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
-    virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
-    virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
-    virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
-    virtual bool checkDerived(std::string prefix="") const = 0;
+// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
+struct ParamsSolver: public ParamsBase
+{
+        std::string prefix = "solver/";
+        double period = 0.0;
+        SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
+
+        ParamsSolver() = default;
+        ParamsSolver(std::string _unique_name, const ParamsServer& _server):
+            ParamsBase(_unique_name, _server)
+        {
+            period  = _server.getParam<double>(prefix + "period");
+            verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
+        }
+        std::string print() const override
+        {
+            return  "period: "                   + std::to_string(period)         + "\n";
+        }
+
+        ~ParamsSolver() override = default;
 };
 
 } // namespace wolf
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index efa0db6d76410f4437180b3bc9f2c844c2b878d5..4e1028bf954a7c2ac71e4d133a097700a6150283 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -116,7 +116,7 @@ inline std::string FactoryStateBlock::getClass() const
 }
 
 template<>
-inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed)
+inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen::VectorXd& _state, bool _fixed)
 {
     typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
 
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index b1af55f7b8c6adafc23d9ef4139b154537f0f9ff..cc4764a4d2452c2494195e47ba892a1ecb0120df 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -16,10 +16,6 @@
 namespace wolf
 {
 
-/// State of nodes containing several state blocks
-typedef std::unordered_map<std::string, Eigen::VectorXd> State;
-//typedef std::string StateStructure;
-
 
 class HasStateBlocks
 {
@@ -29,10 +25,10 @@ class HasStateBlocks
         virtual ~HasStateBlocks();
 
         const StateStructure& getStructure() const { return structure_; }
-        void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
-        bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+        void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
+        bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
 
-        const std::unordered_map<std::string, StateBlockPtr>& getStateBlockMap() const;
+        const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const;
         std::vector<StateBlockPtr> getStateBlockVec() const;
 
         // Some typical shortcuts -- not all should be coded here, see notes below.
@@ -44,27 +40,22 @@ class HasStateBlocks
         virtual void unfix();
         virtual bool isFixed() const;
 
-        virtual StateBlockPtr   addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
-        virtual StateBlockPtr   addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
-        virtual unsigned int    removeStateBlock(const std::string& _sb_type);
-        virtual unsigned int    removeStateBlock(const char _sb_type);
-        bool            hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
-        bool            hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
+        virtual StateBlockPtr   addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
+        virtual unsigned int    removeStateBlock(const char& _sb_type);
+        bool            hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
         bool            hasStateBlock(const StateBlockPtr& _sb) const;
-        StateBlockPtr   getStateBlock(const std::string& _sb_type) const;
-        StateBlockPtr   getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); }
-        bool            setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb);
-        bool            setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); }
-        bool            stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const;
-        std::unordered_map<std::string, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
+        StateBlockPtr   getStateBlock(const char& _sb_type) const;
+        bool            setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
+        bool            stateBlockKey(const StateBlockPtr& _sb, char& _key) const;
+        std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
 
         // Emplace derived state blocks (angle, quaternion, etc).
         template<typename SB, typename ... Args>
-        std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
+        std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
 
         // Emplace base state blocks.
         template<typename ... Args>
-        StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+        StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
 
         // Register/remove state blocks to/from wolf::Problem
         void registerNewStateBlocks(ProblemPtr _problem) const;
@@ -82,12 +73,15 @@ class HasStateBlocks
         unsigned int getLocalSize(const StateStructure& _structure="") const;
 
         // Perturb state
-        void            perturb(double amplitude = 0.01);
+        void perturb(double amplitude = 0.01);
+
+    protected:
+        void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
 
 
     private:
         StateStructure structure_;
-        std::unordered_map<std::string, StateBlockPtr> state_block_map_;
+        std::unordered_map<char, StateBlockPtr> state_block_map_;
 
 };
 
@@ -112,7 +106,7 @@ inline HasStateBlocks::~HasStateBlocks()
     //
 }
 
-inline const std::unordered_map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
 {
     return state_block_map_;
 }
@@ -127,18 +121,18 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
     return sbv;
 }
 
-inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
-{
-    return removeStateBlock(std::string(1, _sb_type));
-}
+//inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
+//{
+//    return removeStateBlock(std::string(1, _sb_type));
+//}
 
-inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type)
+inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
 {
     return state_block_map_.erase(_sb_type);
 }
 
 template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
     std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
@@ -149,7 +143,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string&
 }
 
 template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
     auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
@@ -159,7 +153,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
     return sb;
 }
 
-inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
+inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
 {
     assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
     assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
@@ -167,7 +161,7 @@ inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const Stat
     return true; // success
 }
 
-inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
 {
     auto it = state_block_map_.find(_sb_type);
     if (it != state_block_map_.end())
@@ -178,12 +172,12 @@ inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_
 
 inline wolf::StateBlockPtr HasStateBlocks::getP() const
 {
-    return getStateBlock("P");
+    return getStateBlock('P');
 }
 
 inline wolf::StateBlockPtr HasStateBlocks::getO() const
 {
-    return getStateBlock("O");
+    return getStateBlock('O');
 }
 
 inline void HasStateBlocks::fix()
@@ -220,6 +214,7 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _
         const auto& sb = getStateBlock(key);
         if (!sb)
             WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        assert (sb && "Stateblock key not in the structure");
 
         sb->setState(vec, _notify);
     }
@@ -237,9 +232,8 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char ckey : structure)
+    for (const char key : structure)
     {
-        const auto& key = string(1,ckey);
         const auto& sb  = getStateBlock(key);
         assert (sb && "Stateblock key not in the structure");
 
@@ -256,9 +250,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
 {
     SizeEigen index = 0;
     auto size_it = _sizes.begin();
-    for (const auto& ckey : _structure)
+    for (const auto& key : _structure)
     {
-        const auto& key = string(1,ckey);
         const auto& sb = getStateBlock(key);
         assert (sb && "Stateblock key not in the structure");
         assert(*size_it == sb->getSize() && "State block size mismatch");
@@ -273,9 +266,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
 inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
 {
     auto vec_it = _vectors.begin();
-    for (const auto ckey : _structure)
+    for (const auto key : _structure)
     {
-        const auto key = string(1,ckey);
         const auto& sb = getStateBlock(key);
         assert (sb && "Stateblock key not in the structure");
         assert(vec_it->size() == sb->getSize() && "State block size mismatch");
@@ -316,10 +308,8 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure
 
     VectorComposite state;
 
-    for (const auto ckey : structure)
+    for (const auto key : structure)
     {
-        const auto& key = string(1,ckey); // ckey is char
-
         auto state_it = state_block_map_.find(key);
 
         if (state_it != state_block_map_.end())
@@ -327,10 +317,6 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure
             state.emplace(key, state_it->second->getState());
     }
 
-//    for (auto& pair_key_sb : state_block_map_)
-//    {
-//        state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
-//    }
     return state;
 }
 
@@ -351,11 +337,11 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
     return size;
 }
 
-inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
 {
     const auto& it = std::find_if(state_block_map_.begin(),
                                   state_block_map_.end(),
-                                  [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+                                  [_sb](const std::pair<char, StateBlockPtr>& pair)
                                   {
                                     return pair.second == _sb;
                                   }
@@ -371,7 +357,7 @@ inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
     return it != state_block_map_.end();
 }
 
-inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const
+inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, char& _key) const
 {
     const auto& it = this->find(_sb);
 
@@ -384,7 +370,7 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
     }
     else
     {
-        _key = "";
+        _key = '0'; // '0' = not found
         return false;
     }
 }
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index 2236c1d62e94c6223f13529d2be55d9fd60baf00..460835cb9bf03c92cfb73f8516248816212c6571 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -27,7 +27,7 @@ class LocalParametrizationAngle : public LocalParametrizationBase
         bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
                            Eigen::Map<const Eigen::VectorXd>& _x2,
                            Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
-
+        bool isValid(const Eigen::VectorXd& state, double tolerance) override;
 };
 
 inline LocalParametrizationAngle::LocalParametrizationAngle() :
@@ -64,6 +64,13 @@ inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>&
     return true;
 }
 
+inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
+{
+    //Any real is a valid angle because we use the pi2pi function. Also
+    //the types don't match. In this case the argument is
+    //Eigen::Map not Eigen::VectorXd
+    return true;
+}
 } /* namespace wolf */
 
 #endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 14f2ce38f1dd55b6571884710eb7ed7b7640f985..8494ec3a89215af8287584c8255b241af3486b45 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -31,7 +31,7 @@ class LocalParametrizationBase{
         virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
                            Eigen::Map<const Eigen::VectorXd>& _x2,
                            Eigen::Map<Eigen::VectorXd>&       _x2_minus_x1) = 0;
-
+        virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
         unsigned int getLocalSize() const;
         unsigned int getGlobalSize() const;
 };
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index 30785bb2cae80e94117d6895657b96617b6adefb..b8bbb981c6cec93c9e4fbfa29049a12f16d137a9 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -47,6 +47,7 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
         bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
                            Eigen::Map<const Eigen::VectorXd>& _h2,
                            Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
+        bool isValid(const Eigen::VectorXd& state, double tolerance) override;
 };
 
 } // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 75804f85dabc9d9480ef045944f195fe1a472bb9..c637ab6bb07a23b0e5dd5528d6a1c240327e1473 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -73,6 +73,12 @@ public:
   bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
                      Eigen::Map<const Eigen::VectorXd>& _q2,
                      Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
+  // inline bool isValid(const Eigen::VectorXd& state) override;
+  // template<QuaternionDeltaReference DeltaReference>
+  bool isValid(const Eigen::VectorXd& _state, double tolerance) override
+  {
+      return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
+  }
 };
 
 typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index c8706e2b6d081802d4a3b2590538b2e45df0584b..5406fe0c105c67d87e4793ba86a2e52f81bbfa87 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -159,6 +159,8 @@ public:
 
         void plus(const Eigen::VectorXd& _dv);
 
+        bool isValid(double tolerance = Constants::EPS);
+
         static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
 
 };
@@ -319,7 +321,10 @@ inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fi
 {
     return std::make_shared<StateBlock>(_state, _fixed);
 }
-
+inline bool StateBlock::isValid(double tolerance)
+{
+    return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
+}
 }// namespace wolf
 
 #endif
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 0d3cafcc80c602a4de1bf59b9ce7c2ddd9123c9a..c7a125ac0d96b1d1b0799ab9e983bccf76bf36db 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -22,16 +22,16 @@ using std::string;
 using namespace Eigen;
 
 typedef std::string StateStructure;
-typedef std::unordered_map < std::string, StateBlockPtr   > StateBlockMap;
+typedef std::unordered_map < char, StateBlockPtr   > StateBlockMap;
 typedef StateBlockMap::const_iterator StateBlockMapCIter;
 
-class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd >
+class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
 {
     public:
         VectorComposite() {};
         VectorComposite(const StateStructure& _s);
         VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
-        VectorComposite(const VectorComposite & v) : unordered_map<string, VectorXd>(v){};
+        VectorComposite(const VectorComposite & v) : unordered_map<char, VectorXd>(v){};
         /**
          * \brief Construct from Eigen::VectorXd and structure
          *
@@ -76,17 +76,17 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd
 };
 
 
-class MatrixComposite : public std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >
+class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >
 {
 
     private:
-        std::unordered_map < string, unsigned int> size_rows_, size_cols_;
+        std::unordered_map < char, unsigned int> size_rows_, size_cols_;
         unsigned int    rows() const;
         unsigned int    cols() const;
         void            sizeAndIndices(const StateStructure & _struct_rows,
                                        const StateStructure & _struct_cols,
-                                       std::unordered_map < string, unsigned int>& _indices_rows,
-                                       std::unordered_map < string, unsigned int>& _indices_cols,
+                                       std::unordered_map < char, unsigned int>& _indices_rows,
+                                       std::unordered_map < char, unsigned int>& _indices_cols,
                                        unsigned int& _rows,
                                        unsigned int& _cols) const;
 
@@ -129,25 +129,25 @@ class MatrixComposite : public std::unordered_map < std::string, std::unordered_
         static MatrixComposite identity(const StateStructure& _structure,
                                         const std::list<int>& _sizes);
 
-        unsigned int    count(const std::string &_row,
-                              const std::string &_col) const;
+        unsigned int    count(const char &_row,
+                              const char &_col) const;
 
-        bool            emplace(const std::string &_row,
-                                const std::string &_col,
+        bool            emplace(const char &_row,
+                                const char &_col,
                                 const Eigen::MatrixXd &_mat_blk);
 
         // throw error if queried block is not present
-        bool            at(const std::string &_row,
-                           const std::string &_col,
+        bool            at(const char &_row,
+                           const char &_col,
                            Eigen::MatrixXd &_mat_blk) const;
-        const MatrixXd& at(const std::string &_row,
-                           const std::string &_col) const;
-        MatrixXd&       at(const std::string &_row,
-                           const std::string &_col);
+        const MatrixXd& at(const char &_row,
+                           const char &_col) const;
+        MatrixXd&       at(const char &_row,
+                           const char &_col);
 
         // make some shadowed API available
-        using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::at;
-        using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::count;
+        using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at;
+        using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count;
 
 
         MatrixXd        matrix(const StateStructure & _struct_rows,
@@ -266,25 +266,25 @@ class StateBlockComposite
 
         const StateBlockMap&    getStateBlockMap() const;
 
-        StateBlockPtr           add(const std::string& _sb_type, const StateBlockPtr& _sb);
+        StateBlockPtr           add(const char& _sb_type, const StateBlockPtr& _sb);
 
         // Emplace derived state blocks (angle, quaternion, etc).
         template<typename SB, typename ... Args>
-        std::shared_ptr<SB>     emplace(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+        std::shared_ptr<SB>     emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
 
         // Emplace base state blocks.
         template<typename ... Args>
-        inline StateBlockPtr    emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+        inline StateBlockPtr    emplace(const char& _sb_type, Args&&... _args_of_base_state_block_constructor);
 
-        unsigned int            remove(const std::string& _sb_type);
-        bool                    has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+        unsigned int            remove(const char& _sb_type);
+        bool                    has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
         bool                    has(const StateBlockPtr& _sb) const;
-        StateBlockPtr           at(const std::string& _sb_type) const;
-        void                    set(const std::string& _sb_type, const StateBlockPtr& _sb);
-        void                    set(const std::string& _sb_type, const VectorXd& _vec);
+        StateBlockPtr           at(const char& _sb_type) const;
+        void                    set(const char& _sb_type, const StateBlockPtr& _sb);
+        void                    set(const char& _sb_type, const VectorXd& _vec);
         void                    setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
-        bool                    key(const StateBlockPtr& _sb, std::string& _key) const;
-        std::string             key(const StateBlockPtr& _sb) const;
+        bool                    key(const StateBlockPtr& _sb, char& _key) const;
+        char             key(const StateBlockPtr& _sb) const;
         StateBlockMapCIter      find(const StateBlockPtr& _sb) const;
 
         // identity and zero (they are the same with different names)
@@ -321,7 +321,7 @@ class StateBlockComposite
 
 //////////// IMPLEMENTATION ////////////
 
-inline unsigned int MatrixComposite::count(const std::string &_row, const std::string &_col) const
+inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const
 {
     const auto& rit = this->find(_row);
 
@@ -334,7 +334,7 @@ inline unsigned int MatrixComposite::count(const std::string &_row, const std::s
 
 
 template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_type,
                                                               Args &&... _args_of_derived_state_block_constructor)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
@@ -346,7 +346,7 @@ inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string
 }
 
 template<typename ... Args>
-inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type,
                                                         Args &&... _args_of_base_state_block_constructor)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
@@ -358,7 +358,7 @@ inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_t
 }
 
 inline MatrixComposite::MatrixComposite (const MatrixComposite& m)
-        : unordered_map<string, unordered_map<string, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
+        : unordered_map<char, unordered_map<char, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
 {
 }
 
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 152a7b40e8d154f0b251d8fd322c5f0473126972..23939e2e81229de9bd4ab833ecf86ba2bddfb137 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -6,42 +6,73 @@
 namespace wolf{
 class Problem;
 class FrameBase;
-class TimeStamp;
 }
 
 //Wolf includes
 #include "core/common/wolf.h"
 #include "core/common/node_base.h"
+#include "core/common/time_stamp.h"
 
 //std includes
 
 namespace wolf {
 
+class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
+{
+    public:
+        TrajectoryIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
+            : std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src))
+        {
+
+        }
+
+        // override the indirection operator
+        const FrameBasePtr& operator*() const {
+            return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second;
+        }
+};
+
+class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
+{
+    public:
+        TrajectoryRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
+            : std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src))
+        {
+
+        }
+
+        // override the indirection operator
+        const FrameBasePtr& operator*() const {
+            return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
+        }
+};
+
 //class TrajectoryBase
 class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
 {
     friend FrameBase;
 
     private:
-        std::list<FrameBasePtr> frame_list_;
+         FrameMap frame_map_;
 
     protected:
-        FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
-        FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
+        std::string frame_structure_;  // Defines the structure of the Frames in the Trajectory.
+        // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
+        // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
         
     public:
         TrajectoryBase();
         ~TrajectoryBase() override;
         
         // Frames
-        const FrameBasePtrList& getFrameList() const;
+        const FrameMap& getFrameMap() const;
         FrameBasePtr getLastFrame() const;
-        FrameBasePtr getLastKeyFrame() const;
-        FrameBasePtr getLastKeyOrAuxFrame() const;
-        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
-        FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
-        void sortFrame(FrameBasePtr _frm_ptr);
-        void updateLastFrames();
+        FrameBasePtr getFirstFrame() const;
+        FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+        TrajectoryIter begin() const;
+        TrajectoryIter end() const;
+        TrajectoryRevIter rbegin() const;
+        TrajectoryRevIter rend() const;
 
         virtual void printHeader(int depth, //
                                  bool constr_by, //
@@ -65,30 +96,41 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
     public:
         // factors
         void getFactorList(FactorBasePtrList & _fac_list) const;
-
-    protected:
-        FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr);
-        void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place);
 };
 
-inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
+inline const FrameMap& TrajectoryBase::getFrameMap() const
 {
-    return frame_list_;
+    return frame_map_;
 }
 
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
+inline FrameBasePtr TrajectoryBase::getFirstFrame() const
 {
-    return frame_list_.back();
+    auto it = static_cast<TrajectoryIter>(frame_map_.begin());
+    return *it;
 }
-
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
+inline TrajectoryIter TrajectoryBase::begin() const
+{
+    return static_cast<TrajectoryIter>(frame_map_.begin());
+}
+inline TrajectoryIter TrajectoryBase::end() const
 {
-    return last_key_frame_ptr_;
+    return static_cast<TrajectoryIter>(frame_map_.end());
+}
+inline TrajectoryRevIter TrajectoryBase::rbegin() const
+{
+    return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+}
+inline TrajectoryRevIter TrajectoryBase::rend() const
+{
+    return static_cast<TrajectoryRevIter>(frame_map_.rend());
 }
 
-inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
+inline FrameBasePtr TrajectoryBase::getLastFrame() const
 {
-    return last_key_or_aux_frame_ptr_;
+    // return last_key_frame_ptr_;
+    auto last = this->rbegin();
+    if(last != this->rend()) return *(this->rbegin());
+    else return nullptr;
 }
 
 } // namespace wolf
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 2ffef15edf2e8a719581f76bbd957836ba6dc051..9fd06f2f0d78e82ee1eda5800c10707b4c84b759 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -55,9 +55,9 @@ struct ParamsTreeManagerBase : public ParamsBase
 
     ~ParamsTreeManagerBase() override = default;
 
-    std::string print() const
+    std::string print() const override
     {
-        return ParamsBase::print() + "\n";
+        return "";
     }
 };
 
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 0ff5f31ed2734b7840518bf18dfc32073dac4cec..86bc095f47220e8c82f94c9952eafbe9133cdc96 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -1,7 +1,7 @@
 #ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
 #define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
 
-#include "../tree_manager/tree_manager_base.h"
+#include "core/tree_manager/tree_manager_base.h"
 
 namespace wolf
 {
@@ -15,20 +15,20 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
         ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
             ParamsTreeManagerBase(_unique_name, _server)
         {
-            n_key_frames                = _server.getParam<unsigned int>(prefix + "/n_key_frames");
-            fix_first_key_frame         = _server.getParam<bool>        (prefix + "/fix_first_key_frame");
+            n_frames                    = _server.getParam<unsigned int>(prefix + "/n_frames");
+            fix_first_frame             = _server.getParam<bool>        (prefix + "/fix_first_frame");
             viral_remove_empty_parent   = _server.getParam<bool>        (prefix + "/viral_remove_empty_parent");
         }
         std::string print() const
         {
-            return "\n" + ParamsTreeManagerBase::print()                                            + "\n"
-                        + "n_key_frames: "              + std::to_string(n_key_frames)              + "\n"
-                        + "fix_first_key_frame: "       + std::to_string(fix_first_key_frame)       + "\n"
+            return  ParamsTreeManagerBase::print()                                            + "\n"
+                        + "n_frames: "                  + std::to_string(n_frames)                  + "\n"
+                        + "fix_first_frame: "           + std::to_string(fix_first_frame)           + "\n"
                         + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
         }
 
-        unsigned int n_key_frames;
-        bool fix_first_key_frame;
+        unsigned int n_frames;
+        bool fix_first_frame;
         bool viral_remove_empty_parent;
 };
 
@@ -43,7 +43,7 @@ class TreeManagerSlidingWindow : public TreeManagerBase
 
         ~TreeManagerSlidingWindow() override{}
 
-        void keyFrameCallback(FrameBasePtr _key_frame) override;
+        void keyFrameCallback(FrameBasePtr _frame) override;
 
     protected:
         ParamsTreeManagerSlidingWindowPtr params_sw_;
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d455f89049e94181c762a8a9ef794474f94fd5c
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -0,0 +1,51 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
+
+struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
+{
+        ParamsTreeManagerSlidingWindowDualRate() = default;
+        ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) :
+            ParamsTreeManagerSlidingWindow(_unique_name, _server)
+        {
+            n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent");
+            assert(n_frames_recent <= n_frames);
+            rate_old_frames = _server.getParam<unsigned int>(prefix + "/rate_old_frames");
+        }
+        std::string print() const
+        {
+            return ParamsTreeManagerBase::print()                            + "\n"
+                        + "n_frames_recent: "   + std::to_string(n_frames_recent)   + "\n"
+                        + "rate_old_frames: "   + std::to_string(rate_old_frames)   + "\n";
+        }
+
+        unsigned int n_frames_recent, rate_old_frames;
+};
+
+class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
+{
+    public:
+        TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
+        ;
+        WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+
+        ~TreeManagerSlidingWindowDualRate() override{}
+
+        void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+    protected:
+        ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
+        unsigned int count_frames_;
+        //TrajectoryIter first_recent_frame_it_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ */
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 8c8e34935d728a505537643fa4c51bf001787bb6..0c9502c4ec0cc12ce041611d3a86d5c035af9d09 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -20,9 +20,35 @@
 
 /**
    @file
+   converter.h
+   @brief This file implements a set of simple functions that deal with the correspondence between
+   classes and their string representation. The YAML autosetup framework makes heavy use of this file.
+ */
+
+//Note: In order to deal with string representations we make heavy use of regexps.
+//      We use the standard C++11 regular expressions capabilities.
+
+/*
+** This file is structured essentially in two parts:
+** in the first part (which starts after the CONVERTERS ~~~~ STRING ----> TYPE line)
+** we have functions to convert from string to C++ class. For example:
+     string v1 = "[3,4,5,6,7,8,9,10,11]";
+     vector<int> v = converter<vector<int>>::convert(v1);
+   This code snippet transforms the string v1 which represents an std::vector into an actual std::vector value.
+   The second part (which starts after the TYPES ----> ToSTRING line) has the functions to
+   transform from C++ classes to strings. For instance, if we wanted to convert from a C++ class
+   to a string we would do something like this:
+     vector<int> vect{ 10, 20, 30 };
+     string str = converter<std::string>::convert(vect);
+     //str == "[10,20,30]"
  */
 namespace wolf{
 
+    //This definition is a bit of a "hack". The reason of redefining the pair class is to be able
+    //to have two string representations of a pair, namely
+    //"(·,·)" -> typical pair/tuple representation
+    //"{·,·}" -> key-value pair representation used to represent maps.
+    //This is purely an aesthetic reason and could be removed without problems.
 template<class A, class B>
 struct Wpair : std::pair<A,B>
 {
@@ -94,6 +120,16 @@ struct converter<bool>{
     }
 };
 template<>
+struct converter<char>{
+    static char convert(std::string val){
+        //Here we should check that val.length() == 1 and get val[0] into a char variable
+        if(val.length() == 1) return val.at(0);
+        else throw std::runtime_error("Invalid converstion to char. String too long. String provided: " + val);
+        throw std::runtime_error("Invalid char conversion. String provided: " + val);
+    }
+};
+    //// TYPES ----> ToSTRING
+template<>
 struct converter<std::string>{
     static std::string convert(std::string val){
         return val;
@@ -109,6 +145,9 @@ struct converter<std::string>{
     static std::string convert(double val){
         return std::to_string(val);
     }
+    static std::string convert(char val){
+        return std::to_string(val);
+    }
     template<typename A>
     static std::string convert(utils::list<A> val){
         std::string result = "";
@@ -171,17 +210,17 @@ struct converter<std::string>{
     static std::string convert(VectorComposite val){
         //TODO
         // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
-        auto helper = std::unordered_map<StateStructure, Eigen::VectorXd>();
+        auto helper = std::unordered_map<char, Eigen::VectorXd>();
         for(const auto& pair: val)
-            helper.insert(std::pair<StateStructure, Eigen::VectorXd>(pair.first, pair.second));
+            helper.insert(std::pair<char, Eigen::VectorXd>(pair.first, pair.second));
         return converter<std::string>::convert(helper);
     }
     static std::string convert(MatrixComposite val){
         //TODO
         // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
-        auto helper = std::unordered_map< StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>();
+        auto helper = std::unordered_map< char, std::unordered_map<char, Eigen::MatrixXd>>();
         for(const auto& pair: val)
-            helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(pair.first, pair.second));
+            helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(pair.first, pair.second));
         return converter<std::string>::convert(helper);
     }
 };
@@ -305,13 +344,13 @@ struct converter<std::unordered_map<A,B>>{
 template<>
 struct converter<VectorComposite>{
     static VectorComposite convert(std::string val){
-       auto unordered_map = converter<std::unordered_map<StateStructure, Eigen::VectorXd>>::convert(val);
+       auto unordered_map = converter<std::unordered_map<char, Eigen::VectorXd>>::convert(val);
     //    VectorComposite result = VectorComposite(unordered_map);
     //    return result;
        auto helper = VectorComposite();
        for(auto const& it: unordered_map)
        {
-           helper.insert(std::pair<StateStructure, Eigen::VectorXd>(it.first, it.second));
+           helper.insert(std::pair<char, Eigen::VectorXd>(it.first, it.second));
        }
         return helper;
     }
@@ -319,14 +358,14 @@ struct converter<VectorComposite>{
 template<>
 struct converter<MatrixComposite>{
     static MatrixComposite convert(std::string val){
-       auto unordered_map = converter<std::unordered_map<StateStructure,
-                                                         std::unordered_map<StateStructure, Eigen::MatrixXd>>>::convert(val);
+       auto unordered_map = converter<std::unordered_map<char,
+                                                         std::unordered_map<char, Eigen::MatrixXd>>>::convert(val);
     //    VectorComposite result = VectorComposite(unordered_map);
     //    return result;
        auto helper = MatrixComposite();
        for(auto const& it: unordered_map)
        {
-           helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(it.first, it.second));
+           helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(it.first, it.second));
        }
         return helper;
     }
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 880af68b86919dbab088f7f702f07ac41feefb98..f29109d1862d134aa592df4b4c843cac6b0a8bd0 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -23,26 +23,26 @@ CaptureBase::CaptureBase(const std::string& _type,
     assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!");
     if (_sensor_ptr)
     {
-        if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
+        if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
         {
             assert(_p_ptr && "Pointer to dynamic position params is null!");
-            addStateBlock("P", _p_ptr, nullptr);
+            addStateBlock('P', _p_ptr, nullptr);
         }
         else
             assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
 
-        if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
+        if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
         {
             assert(_o_ptr && "Pointer to dynamic orientation params is null!");
-            addStateBlock("O", _o_ptr, nullptr);
+            addStateBlock('O', _o_ptr, nullptr);
         }
         else
             assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
 
-        if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
+        if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
         {
             assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
-            addStateBlock("I", _intr_ptr, nullptr);
+            addStateBlock('I', _intr_ptr, nullptr);
         }
         else
             assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
@@ -56,7 +56,6 @@ CaptureBase::CaptureBase(const std::string& _type,
 
 CaptureBase::~CaptureBase()
 {
-    removeStateBlocks(getProblem());
 }
 
 void CaptureBase::remove(bool viral_remove_empty_parent)
@@ -71,6 +70,10 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
         is_removing_ = true;
         CaptureBasePtr this_C = shared_from_this();  // keep this alive while removing it
 
+        // SensorBase::last_capture_
+        if (getSensor() and getSensor()->getLastCapture() == this_C)
+            getSensor()->updateLastCapture();
+
         // remove downstream
         while (!constrained_by_list_.empty())
         {
@@ -149,15 +152,7 @@ bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
 }
 
 
-const StateStructure& CaptureBase::getStructure() const
-{
-    if (getSensor())
-        return getSensor()->getStructure();
-    else
-        return HasStateBlocks::getStructure();
-}
-
-StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
+StateBlockPtr CaptureBase::getStateBlock(const char& _key) const
 {
     if (getSensor() and getSensor()->hasStateBlock(_key))
     {
@@ -182,11 +177,9 @@ void CaptureBase::unfix()
 
 void CaptureBase::move(FrameBasePtr _frm_ptr)
 {
-    WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
-    WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr");
+    WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!");
 
-    assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF");
-    assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame");
+    assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!");
 
     // Unlink
     if (this->getFrame())
@@ -215,7 +208,7 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
     }
     else
     {
-        WOLF_WARN("Linking with a nullptr");
+        WOLF_WARN("Linking Capture ", id(), " to a nullptr");
     }
 }
 
@@ -224,16 +217,26 @@ void CaptureBase::setProblem(ProblemPtr _problem)
     if (_problem == nullptr || _problem == this->getProblem())
         return;
 
+    assert(getFrame() && "Cannot set problem: Capture has no Frame!");
+
     NodeBase::setProblem(_problem);
     registerNewStateBlocks(_problem);
 
+    // update SensorBase::last_capture_
+    if (getSensor() and
+        (not getSensor()->getLastCapture() or
+         getSensor()->getLastCapture()->getTimeStamp() < time_stamp_))
+         getSensor()->setLastCapture(shared_from_this());
+
     for (auto ft : feature_list_)
         ft->setProblem(_problem);
 }
 
 void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
-    _stream << _tabs << "Cap" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
+    _stream << _tabs << "Cap" << id()
+            << " " << getType()
+            << " ts = " << std::setprecision(3) << getTimeStamp();
 
     if(getSensor() != nullptr)
     {
@@ -247,36 +250,13 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
     {
         _stream << "  <-- ";
         for (auto cby : getConstrainedByList())
-            _stream << "Fac" << cby->id() << " \t";
+            if (cby)
+                _stream << "Fac" << cby->id() << " \t";
     }
     _stream << std::endl;
 
-    if(getStateBlockMap().size() > 0)
-    {
-        if (_metric && _state_blocks){
-            for (const auto& key : getStructure())
-            {
-                auto sb = getStateBlock(key);
-                _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
-            }
-        }
-        else if (_metric)
-        {
-            _stream << _tabs << (isFixed() ? "Fix" : "Est");
-            _stream << ",\t x= ( " << std::setprecision(2) << getStateVector() << " )";
-            _stream << std::endl;
-        }
-        else if (_state_blocks)
-        {
-            _stream << "    sb:";
-            for (const auto& key : getStructure())
-            {
-                const auto& sb = getStateBlock(key);
-                _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
-            }
-            _stream << std::endl;
-        }
-    }
+    if (getStateBlockMap().size() > 0)
+        printState(_metric, _state_blocks, _stream, _tabs);
 }
 
 void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -284,7 +264,8 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 3)
         for (auto f : getFeatureList())
-            f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (f)
+                f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
@@ -364,7 +345,7 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
         auto frm_cap = _cap_ptr->getFrame();
         inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
                                   << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
-                                  << " -X-> Frm" << id();
+                                  << " -X-> Frm" << id() << "\n";
         auto frm_cap_list = frm_cap->getCaptureList();
         auto frame_has_cap = std::find_if(frm_cap_list.begin(), frm_cap_list.end(), [&_cap_ptr](CaptureBasePtr cap){ return cap == _cap_ptr;});
         log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
@@ -376,6 +357,28 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
                                       << " -X-> Cap" << id();
             log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
         }
+        //Check that the capture is within time tolerance of some processor
+        auto frame = getFrame();
+        double time_diff = fabs(getTimeStamp() - frame->getTimeStamp());
+
+        //It looks like some gtests add captures by hand, without using processors, so the processor list is empty.
+        //This inicialization is needed because if the list empty the execution does not go into the loop and the
+        //assertion fails
+        bool match_any_prc_timetolerance;
+        if(getSensor() != nullptr )
+        {
+            match_any_prc_timetolerance = getSensor()->getProcessorList().empty();
+            for(auto const& prc : getSensor()->getProcessorList())
+            {
+                match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
+            }
+            inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+                                      << " ts =" << getTimeStamp() << "Frm" << frame->id()
+                                      << " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
+                                      << " any processor in sensor " << getSensor()->id() << "\n";
+            log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
+        }
+
     return log;
 }
 bool CaptureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index da17a57af80971b21f4a69322ba0c2ade54225d4..16a535dc987deea66756f4c8468ac3005f2b3573 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -75,7 +75,9 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera
 
 void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
-    _stream << _tabs << "CapM" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
+    _stream << _tabs << "CapM" << id()
+            << " " << getType()
+            << " ts = " << std::setprecision(3) << getTimeStamp();
 
     if(getSensor() != nullptr)
     {
@@ -83,11 +85,12 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
     }
     else
         _stream << " -> Sen-";
+
     if (auto OC = getOriginCapture())
     {
-        _stream << " -> OCap" << OC->id();
+        _stream << " -> Origin: Cap" << OC->id();
         if (auto OF = OC->getFrame())
-            _stream << " ; OFrm" << OF->id();
+            _stream << " ; Frm" << OF->id();
     }
 
     _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
@@ -99,18 +102,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
     }
     _stream << std::endl;
 
-    if (_state_blocks)
-        for (const auto& sb : getStateBlockVec())
-        {
-            if(sb != nullptr)
-            {
-                _stream << _tabs << "  " << "sb: ";
-                _stream << (sb->isFixed() ? "Fix" : "Est");
-                if (_metric)
-                    _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
-                _stream << std::endl;
-            }
-        }
+    if (getStateBlockMap().size() > 0)
+        printState(_metric, _state_blocks, _stream, _tabs);
 
     _stream << _tabs << "  " << "buffer size  :  " << getBuffer().size() << std::endl;
     if ( _metric && ! getBuffer().empty())
@@ -120,8 +113,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
 //        {
 //            _stream << _tabs << "  " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
 //            _stream << _tabs << "  " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
-////            _stream << _tabs << "  " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
-////            _stream << _tabs << "  " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+//            _stream << _tabs << "  " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
+//            _stream << _tabs << "  " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
 //        }
     }
 
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/solver_ceres.cpp
similarity index 63%
rename from src/ceres_wrapper/ceres_manager.cpp
rename to src/ceres_wrapper/solver_ceres.cpp
index b5bcfa4b7b6dc09e990adbecb93685c0294dbef5..67b009f243ace9d81664709c375a2008351cb73f 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -1,40 +1,36 @@
-#include "core/ceres_wrapper/ceres_manager.h"
+#include "core/ceres_wrapper/solver_ceres.h"
 #include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
+#include "core/ceres_wrapper/cost_function_wrapper.h"
+#include "core/ceres_wrapper/iteration_update_callback.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
 #include "core/trajectory/trajectory_base.h"
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
 #include "core/utils/make_unique.h"
 
-namespace wolf {
+namespace wolf
+{
+
+SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) :
+        SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>())
+{
+}
 
-CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
-                           const ceres::Solver::Options& _ceres_options) :
-    SolverManager(_wolf_problem),
-    ceres_options_(_ceres_options)
+SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
+                         const ParamsCeresPtr& _params)
+     : SolverManager(_wolf_problem, _params)
+     , params_ceres_(_params)
 {
-    ceres::Covariance::Options covariance_options;
-#if CERES_VERSION_MINOR >= 13
-    covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
-    covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
-#elif CERES_VERSION_MINOR >= 10
-    covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
-#else
-    covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
-#endif
-    covariance_options.num_threads = 1;
-    covariance_options.apply_loss_function = false;
-    //covariance_options.null_space_rank = -1;
-    covariance_ = wolf::make_unique<ceres::Covariance>(covariance_options);
-
-    ceres::Problem::Options problem_options;
-    problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-    problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
-    problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-
-    ceres_problem_ = wolf::make_unique<ceres::Problem>(problem_options);
+    covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
+    ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
+
+    if (params_ceres_->update_immediately)
+        getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
+                                                                           params_ceres_->min_num_iterations,
+                                                                           params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
 }
 
-CeresManager::~CeresManager()
+SolverCeres::~SolverCeres()
 {
     while (!fac_2_residual_idx_.empty())
         removeFactorDerived(fac_2_residual_idx_.begin()->first);
@@ -44,21 +40,37 @@ CeresManager::~CeresManager()
         removeStateBlockDerived(state_blocks_.begin()->first);
         state_blocks_.erase(state_blocks_.begin());
     }
-}
 
-    SolverManagerPtr CeresManager::create(const ProblemPtr &_wolf_problem, const ParamsServer& _server)
-{
-    ceres::Solver::Options opt;
-    opt.max_num_iterations = _server.getParam<int>("solver/max_num_iterations");
-    // CeresManager m = CeresManager(wolf_problem, opt);
-    return std::make_shared<CeresManager>(_wolf_problem, opt);
+    while (!getSolverOptions().callbacks.empty())
+    {
+        delete getSolverOptions().callbacks.back();
+        getSolverOptions().callbacks.pop_back();
+    }
 }
 
-std::string CeresManager::solveDerived(const ReportVerbosity report_level)
+std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
 {
     // run Ceres Solver
-    ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
+    ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
+
+    /*/ if termination by user (Iteration callback, update and solve again)
+    // solve until max iterations reached
+    if (params_ceres_->update_immediately)
+    {
+        auto max_num_iterations = getSolverOptions().max_num_iterations;
+        while (summary_.termination_type == ceres::USER_SUCCESS)
+        {
+            // decrease wasted iterations
+            getSolverOptions().max_num_iterations -= summary_.iterations.size();
+            if (getSolverOptions().max_num_iterations <= 0)
+                break;
 
+            // update and solve again
+            update();
+            ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
+        }
+        getSolverOptions().max_num_iterations = max_num_iterations;
+    }*/
     std::string report;
 
     //return report
@@ -70,14 +82,11 @@ std::string CeresManager::solveDerived(const ReportVerbosity report_level)
     return report;
 }
 
-void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
+bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
 {   
     // update problem
     update();
 
-    // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
-    wolf_problem_->clearCovariance();
-
     // CREATE DESIRED COVARIANCES LIST
     std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
     std::vector<std::pair<const double*, const double*>> double_pairs;
@@ -89,13 +98,12 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             // first create a vector containing all state blocks
             std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
             //frame state blocks
-            for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
-                if (fr_ptr->isKeyOrAux())
-                    for (const auto& key : fr_ptr->getStructure())
-                    {
-                        const auto& sb = fr_ptr->getStateBlock(key);
-                        all_state_blocks.push_back(sb);
-                    }
+            for(auto fr_ptr : *wolf_problem_->getTrajectory())
+                for (const auto& key : fr_ptr->getStructure())
+                {
+                    const auto& sb = fr_ptr->getStateBlock(key);
+                    all_state_blocks.push_back(sb);
+                }
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
@@ -105,40 +113,54 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             }
             // double loop all against all (without repetitions)
             for (unsigned int i = 0; i < all_state_blocks.size(); i++)
+            {
+                assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
                 for  (unsigned int j = i; j < all_state_blocks.size(); j++)
                 {
+                    assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
+
                     state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
                     double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
                                               getAssociatedMemBlockPtr(all_state_blocks[j]));
                 }
+            }
             break;
         }
         case CovarianceBlocksToBeComputed::ALL_MARGINALS:
         {
             // first create a vector containing all state blocks
-            for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
-                if (fr_ptr->isKeyOrAux())
-                    for (const auto& key1 : wolf_problem_->getFrameStructure())
-                        for (const auto& key2 : wolf_problem_->getFrameStructure())
-                        {
-                            const auto& sb1 = fr_ptr->getStateBlock(key1);
-                            const auto& sb2 = fr_ptr->getStateBlock(key2);
-                            state_block_pairs.emplace_back(sb1, sb2);
-                            double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
-                            if (sb1 == sb2)
-                                break;
-                        }
+            for(auto fr_ptr : *wolf_problem_->getTrajectory())
+                for (const auto& key1 : wolf_problem_->getFrameStructure())
+                {
+                    const auto& sb1 = fr_ptr->getStateBlock(key1);
+                    assert(isStateBlockRegisteredDerived(sb1));
+
+                    for (const auto& key2 : wolf_problem_->getFrameStructure())
+                    {
+                        const auto& sb2 = fr_ptr->getStateBlock(key2);
+                        assert(isStateBlockRegisteredDerived(sb2));
+
+                        state_block_pairs.emplace_back(sb1, sb2);
+                        double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+                        if (sb1 == sb2)
+                            break;
+                    }
+                }
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
                 for (auto sb : l_ptr->getUsedStateBlockVec())
+                {
+                    assert(isStateBlockRegisteredDerived(sb));
                     for(auto sb2 : l_ptr->getUsedStateBlockVec())
                     {
+                        assert(isStateBlockRegisteredDerived(sb2));
                         state_block_pairs.emplace_back(sb, sb2);
                         double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
                         if (sb == sb2)
                             break;
                     }
+                }
             //            // loop all marginals (PO marginals)
             //            for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
             //            {
@@ -155,7 +177,10 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
         case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
         {
             //robot-robot
-            auto last_key_frame = wolf_problem_->getLastKeyFrame();
+            auto last_key_frame = wolf_problem_->getLastFrame();
+
+            assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
+            assert(isStateBlockRegisteredDerived(last_key_frame->getO()));
 
             state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
             state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
@@ -177,6 +202,8 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
 
                 for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
                 {
+                    assert(isStateBlockRegisteredDerived(*state_it));
+
                     // robot - landmark
                     state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
                     state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
@@ -188,20 +215,65 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
                     // landmark marginal
                     for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
                     {
-                      state_block_pairs.emplace_back(*state_it, *next_state_it);
-                      double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
+                        assert(isStateBlockRegisteredDerived(*next_state_it));
+
+                        state_block_pairs.emplace_back(*state_it, *next_state_it);
+                        double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
                                                 getAssociatedMemBlockPtr((*next_state_it)));
                     }
                 }
             }
             break;
         }
+        case CovarianceBlocksToBeComputed::GAUSS:
+        {
+            // State blocks:
+            // - Last KF: PV
+
+            // last KF
+            FrameBasePtr last_frame = wolf_problem_->getLastFrame();
+            if (not last_frame)
+                return false;
+
+            // Error
+            if (not last_frame->hasStateBlock('P') or
+                not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or
+                not last_frame->hasStateBlock('V') or
+                not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')))
+            {
+                WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P or V, returning...");
+                WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found");
+                WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
+                WOLF_WARN_COND(not last_frame->getStateBlock('V'), "SolverCeres::computeCovariances: KF state block 'V' not found");
+                WOLF_WARN_COND(last_frame->getStateBlock('V') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')), "SolverCeres::computeCovariances: KF state block 'V' not registered ", last_frame->getStateBlock('V'));
+
+                return false;
+            }
+
+            // only marginals of P and V
+            state_block_pairs.emplace_back(last_frame->getStateBlock('P'),
+                                           last_frame->getStateBlock('P'));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('P')),
+                                      getAssociatedMemBlockPtr(last_frame->getStateBlock('P')));
+            state_block_pairs.emplace_back(last_frame->getStateBlock('V'),
+                                           last_frame->getStateBlock('V'));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')),
+                                      getAssociatedMemBlockPtr(last_frame->getStateBlock('V')));
+
+            break;
+        }
+        default:
+            throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
+
     }
     //std::cout << "pairs... " << double_pairs.size() << std::endl;
 
     // COMPUTE DESIRED COVARIANCES
     if (covariance_->Compute(double_pairs, ceres_problem_.get()))
     {
+        // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+        wolf_problem_->clearCovariance();
+
         // STORE DESIRED COVARIANCES
         for (unsigned int i = 0; i < double_pairs.size(); i++)
         {
@@ -210,34 +282,39 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
             // std::cout << "covariance got switch: " << std::endl << cov << std::endl;
         }
+        return true;
     }
-    else
-        std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+
+    WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
+    return false;
 }
-void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+
+bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list)
 {
-    //std::cout << "CeresManager: computing covariances..." << std::endl;
+    //std::cout << "SolverCeres: computing covariances..." << std::endl;
 
     // update problem
     update();
 
-    // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
-    wolf_problem_->clearCovariance();
-
     // CREATE DESIRED COVARIANCES LIST
     std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
     std::vector<std::pair<const double*, const double*>> double_pairs;
 
     // double loop all against all (without repetitions)
-    for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++){
+    for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
+    {
         if (*st_it1 == nullptr){
             continue;
         }
+        assert(isStateBlockRegisteredDerived(*st_it1));
+
         for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
         {
             if (*st_it2 == nullptr){
                 continue;
             }
+            assert(isStateBlockRegisteredDerived(*st_it2));
+
             state_block_pairs.emplace_back(*st_it1, *st_it2);
             double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
                                       getAssociatedMemBlockPtr((*st_it2)));
@@ -248,6 +325,10 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
 
     // COMPUTE DESIRED COVARIANCES
     if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+    {
+        // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+        wolf_problem_->clearCovariance();
+
         // STORE DESIRED COVARIANCES
         for (unsigned int i = 0; i < double_pairs.size(); i++)
         {
@@ -256,13 +337,16 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
             wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
             // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
         }
-    else
-        std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+        return true;
+    }
+
+    WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
+    return false;
 }
 
-void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr)
+void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
 {
-    assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
+    assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
 
     auto cost_func_ptr = createCostFunction(fac_ptr);
     fac_2_costfunction_[fac_ptr] = cost_func_ptr;
@@ -271,6 +355,7 @@ void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr)
     res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
     for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
     {
+        assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
         res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
     }
 
@@ -288,7 +373,7 @@ void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr)
     assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
 }
 
-void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr)
+void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
 {
     assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
 
@@ -299,10 +384,10 @@ void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr)
     assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
 }
 
-void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr)
+void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
 {
     assert(state_ptr);
-    assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added");
+    assert(!isStateBlockRegisteredDerived(state_ptr) && "adding a state block already added");
     assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
 
     ceres::LocalParameterization* local_parametrization_ptr = nullptr;
@@ -325,25 +410,28 @@ void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr)
     updateStateBlockStatusDerived(state_ptr);
 }
 
-void CeresManager::removeStateBlockDerived(const StateBlockPtr& state_ptr)
+void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
 {
-    //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
+    //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
     assert(state_ptr);
-    assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres");
+    assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
+
     ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
     state_blocks_local_param_.erase(state_ptr);
 }
 
-void CeresManager::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
+void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
 {
     assert(state_ptr != nullptr);
+    assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres");
+
     if (state_ptr->isFixed())
         ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
     else
         ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
 }
 
-void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr)
+void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr)
 {
     assert(state_ptr != nullptr);
 
@@ -379,27 +467,27 @@ void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockP
         addFactorDerived(fac);
 }
 
-bool CeresManager::hasConverged()
+bool SolverCeres::hasConverged()
 {
     return summary_.termination_type == ceres::CONVERGENCE;
 }
 
-SizeStd CeresManager::iterations()
+SizeStd SolverCeres::iterations()
 {
     return summary_.iterations.size();
 }
 
-double CeresManager::initialCost()
+double SolverCeres::initialCost()
 {
     return double(summary_.initial_cost);
 }
 
-double CeresManager::finalCost()
+double SolverCeres::finalCost()
 {
     return double(summary_.final_cost);
 }
 
-ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fac_ptr)
+ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac_ptr)
 {
     assert(_fac_ptr != nullptr);
 
@@ -415,7 +503,7 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa
         throw std::invalid_argument( "Wrong Jacobian Method!" );
 }
 
-bool CeresManager::checkDerived(std::string prefix) const
+bool SolverCeres::checkDerived(std::string prefix) const
 {
     bool ok = true;
 
@@ -424,12 +512,12 @@ bool CeresManager::checkDerived(std::string prefix) const
         ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or
         ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
     {
-        WOLF_ERROR("CeresManager::check: number of residuals mismatch  - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
+        WOLF_ERROR("SolverCeres::check: number of residuals mismatch  - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
         ok = false;
     }
     if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
     {
-        WOLF_ERROR("CeresManager::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
+        WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
         ok = false;
     }
 
@@ -439,7 +527,7 @@ bool CeresManager::checkDerived(std::string prefix) const
     {
         if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
         {
-            WOLF_ERROR("CeresManager::check: any state block is missing in ceres problem - in ", prefix);
+            WOLF_ERROR("SolverCeres::check: any state block is missing in ceres problem - in ", prefix);
             ok = false;
         }
     }
@@ -450,27 +538,27 @@ bool CeresManager::checkDerived(std::string prefix) const
         // SolverManager::factors_
         if (factors_.count(fac_res_pair.first) == 0)
         {
-            WOLF_ERROR("CeresManager::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
+            WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
             ok = false;
         }
 
         // costfunction - residual
         if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
         {
-            WOLF_ERROR("CeresManager::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
+            WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
             ok = false;
         }
         //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
         if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
         {
-            WOLF_ERROR("CeresManager::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
+            WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
             ok = false;
         }
 
         // factor - residual
         if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
         {
-            WOLF_ERROR("CeresManager::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
+            WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
             ok = false;
         }
 
@@ -479,7 +567,7 @@ bool CeresManager::checkDerived(std::string prefix) const
         ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, &param_blocks);
         if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size())
         {
-            WOLF_ERROR("CeresManager::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
+            WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
             ok = false;
         }
         else
@@ -489,7 +577,7 @@ bool CeresManager::checkDerived(std::string prefix) const
             {
                 if (getAssociatedMemBlockPtr(st) != param_blocks[i])
                 {
-                    WOLF_ERROR("CeresManager::check: parameter", i, " mismatch - in ", prefix);
+                    WOLF_ERROR("SolverCeres::check: parameter", i, " mismatch - in ", prefix);
                     ok = false;
                 }
                 i++;
@@ -508,7 +596,7 @@ void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& origina
     original.makeCompressed();
 }
 
-const Eigen::SparseMatrixd CeresManager::computeHessian() const
+const Eigen::SparseMatrixd SolverCeres::computeHessian() const
 {
     Eigen::SparseMatrixd H;
     Eigen::SparseMatrixd A;
@@ -539,7 +627,7 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const
             StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
             if (!sb->isFixed())
             {
-                assert(state_blocks_local_param_.count(sb) != 0 && "factor involving a state block not added");
+                assert(state_blocks_.count(sb) != 0 && "factor involving a state block not added");
                 assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
 
                 // insert since A_block_row has just been created so it's empty for sure
@@ -569,10 +657,18 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const
     return H;
 }
 
+bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                     const LocalParametrizationBasePtr& local_param)
+{
+    return state_blocks_local_param_.count(st) == 1
+            && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
+            && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
+                    == state_blocks_local_param_.at(st).get();
+}
 
 } // namespace wolf
 #include "core/solver/factory_solver.h"
 namespace wolf {
-    WOLF_REGISTER_SOLVER(CeresManager)
+    WOLF_REGISTER_SOLVER(SolverCeres)
 } // namespace wolf
 
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 4e18cb47d8485a7b9ee83853e863ddf20bb7e78a..c03fe00ad9b3a6018747b07572d0d6c1dbfd4a56 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -151,20 +151,23 @@ void FactorBase::remove(bool viral_remove_empty_parent)
 
 CaptureBasePtr FactorBase::getCapture() const
 {
-    assert(getFeature() != nullptr && "calling getCapture before linking with a feature");
-    return getFeature()->getCapture();
+    auto ftr = getFeature();
+    if (ftr != nullptr) return ftr->getCapture();
+    else return nullptr;
 }
 
 FrameBasePtr FactorBase::getFrame() const
 {
-    assert(getCapture() != nullptr && "calling getFrame before linking with a capture");
-    return getCapture()->getFrame();
+    auto cpt = getCapture();
+    if(cpt != nullptr) return cpt->getFrame();
+    else return nullptr;
 }
 
 SensorBasePtr FactorBase::getSensor() const
 {
-    assert(getCapture() != nullptr && "calling getSensor before linking with a capture");
-    return getCapture()->getSensor();
+    auto cpt = getCapture();
+    if(cpt != nullptr) return cpt->getSensor();
+    else return nullptr;
 }
 
 void FactorBase::setStatus(FactorStatus _status)
@@ -257,10 +260,6 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
     _ftr_ptr->addFactor(shared_from_this());
     this->setFeature(_ftr_ptr);
 
-    // if frame, should be key
-    if (getCapture() and getFrame())
-        assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
-
     // set problem ( and register factor )
     WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
     this->setProblem(_ftr_ptr->getProblem());
@@ -271,7 +270,7 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
         auto frame_other = frm_ow.lock();
         if(frame_other != nullptr)
         {
-            assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other.");
+            assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other.");
             frame_other->addConstrainedBy(shared_from_this());
         }
     }
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 69133345e0f57c28e3135aa3b50e0d99e179be50..c2af9127c6a2e028d98f4f5413490117bfbd7825 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -182,7 +182,8 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
     {
         _stream << "  <--\t";
         for (auto cby : getConstrainedByList())
-            _stream << "Fac" << cby->id() << " \t";
+            if (cby)
+                _stream << "Fac" << cby->id() << " \t";
     }
     _stream << std::endl;
     if (_metric)
@@ -196,7 +197,8 @@ void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 4)
         for (auto c : getFactorList())
-            c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (c)
+                c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 4147b98ca8dcd67d76fb732f11cbbee85cea9df8..515de7b3de60ac3d9ad8fa5d48b58ba9b6939165 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -12,15 +12,13 @@ namespace wolf {
 
 unsigned int FrameBase::frame_id_count_ = 0;
 
-FrameBase::FrameBase(const FrameType & _tp,
-                     const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
                      const std::string _frame_structure,
                      const VectorComposite& _state) :
         NodeBase("FRAME", "FrameBase"),
         HasStateBlocks(_frame_structure),
         trajectory_ptr_(),
         frame_id_(++frame_id_count_),
-        type_(_tp),
         time_stamp_(_ts)
 {
     for (const auto& pair_key_vec : _state)
@@ -28,33 +26,30 @@ FrameBase::FrameBase(const FrameType & _tp,
         const auto& key  = pair_key_vec.first;
         const auto& vec  = pair_key_vec.second;
 
-        StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
+        StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
 
         addStateBlock(key, sb);
     }
 }
 
 
-FrameBase::FrameBase(const FrameType & _tp,
-                     const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
                      const std::string _frame_structure,
                      const std::list<VectorXd>& _vectors) :
                 NodeBase("FRAME", "FrameBase"),
                 HasStateBlocks(_frame_structure),
                 trajectory_ptr_(),
                 frame_id_(++frame_id_count_),
-                type_(_tp),
                 time_stamp_(_ts)
 {
     assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
 
     auto vec_it = _vectors.begin();
-    for (const auto ckey : _frame_structure)
+    for (const auto key : _frame_structure)
     {
-        const auto& key = string(1,ckey);
         const auto& vec = *vec_it;
 
-        StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
+        StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
 
         addStateBlock(key, sb);
 
@@ -63,8 +58,7 @@ FrameBase::FrameBase(const FrameType & _tp,
 }
 
 
-FrameBase::FrameBase(const FrameType & _tp,
-                     const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
                      StateBlockPtr _p_ptr,
                      StateBlockPtr _o_ptr,
                      StateBlockPtr _v_ptr) :
@@ -72,20 +66,19 @@ FrameBase::FrameBase(const FrameType & _tp,
             HasStateBlocks(""),
             trajectory_ptr_(),
             frame_id_(++frame_id_count_),
-            type_(_tp),
             time_stamp_(_ts)
 {
     if (_p_ptr)
     {
-        addStateBlock("P", _p_ptr);
+        addStateBlock('P', _p_ptr);
     }
     if (_o_ptr)
     {
-        addStateBlock("O", _o_ptr);
+        addStateBlock('O', _o_ptr);
     }
     if (_v_ptr)
     {
-        addStateBlock("V", _v_ptr);
+        addStateBlock('V', _v_ptr);
     }
 }
 
@@ -117,8 +110,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
         }
 
         // Remove Frame State Blocks
-        if ( isKeyOrAux() )
-            removeStateBlocks(getProblem());
+        removeStateBlocks(getProblem());
 
         // remove from upstream
         TrajectoryBasePtr T = trajectory_ptr_.lock();
@@ -132,52 +124,12 @@ void FrameBase::remove(bool viral_remove_empty_parent)
 void FrameBase::setTimeStamp(const TimeStamp& _ts)
 {
     time_stamp_ = _ts;
-    if (isKeyOrAux() && getTrajectory() != nullptr)
-        getTrajectory()->sortFrame(shared_from_this());
 }
 
-void FrameBase::setNonEstimated()
+void FrameBase::link(ProblemPtr _prb)
 {
-    // unregister if previously estimated
-    if (isKeyOrAux())
-        for (const auto& sb : getStateBlockVec())
-            getProblem()->notifyStateBlock(sb, REMOVE);
-
-    type_ = NON_ESTIMATED;
-    if (getTrajectory())
-    {
-        getTrajectory()->sortFrame(shared_from_this());
-        getTrajectory()->updateLastFrames();
-    }
-}
-
-void FrameBase::setKey()
-{
-    // register if previously not estimated
-    if (!isKeyOrAux())
-        registerNewStateBlocks(getProblem());
-
-    type_ = KEY;
-
-    if (getTrajectory())
-    {
-        getTrajectory()->sortFrame(shared_from_this());
-        getTrajectory()->updateLastFrames();
-    }
-}
-
-void FrameBase::setAux()
-{
-    if (!isKeyOrAux())
-        registerNewStateBlocks(getProblem());
-
-    type_ = AUXILIARY;
-
-    if (getTrajectory())
-    {
-        getTrajectory()->sortFrame(shared_from_this());
-        getTrajectory()->updateLastFrames();
-    }
+    assert(_prb != nullptr && "Trying to link Frame with a null problem pointer!");
+    this->link(_prb->getTrajectory());
 }
 
 bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
@@ -190,12 +142,12 @@ FrameBasePtr FrameBase::getPreviousFrame() const
     assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
 
     //look for the position of this node in the upper list (frame list of trajectory)
-    for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
+    for (auto f_it = getTrajectory()->rbegin(); f_it != getTrajectory()->rend(); f_it++ )
     {
         if ( this->frame_id_ == (*f_it)->id() )
         {
         	f_it++;
-        	if (f_it != getTrajectory()->getFrameList().rend())
+        	if (f_it != getTrajectory()->rend())
             {
                 return *f_it;
             }
@@ -211,11 +163,11 @@ FrameBasePtr FrameBase::getPreviousFrame() const
 FrameBasePtr FrameBase::getNextFrame() const
 {
     //std::cout << "finding next frame of " << this->frame_id_ << std::endl;
-	auto f_it = getTrajectory()->getFrameList().rbegin();
+	auto f_it = getTrajectory()->rbegin();
 	f_it++; //starting from second last frame
 
     //look for the position of this node in the frame list of trajectory
-    while (f_it != getTrajectory()->getFrameList().rend())
+    while (f_it != getTrajectory()->rend())
     {
         if ( this->frame_id_ == (*f_it)->id())
         {
@@ -326,7 +278,7 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
     }
     else
     {
-        WOLF_WARN("Linking with a nullptr");
+        WOLF_WARN("Linking Frame ", id(), " to a nullptr");
     }
 }
 
@@ -336,8 +288,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
         return;
 
     NodeBase::setProblem(_problem);
-    if (this->isKey())
-        registerNewStateBlocks(_problem);
+    registerNewStateBlocks(_problem);
 
     for (auto cap : capture_list_)
         cap->setProblem(_problem);
@@ -345,7 +296,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
 
 void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
-    _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id()
+    _stream << _tabs << "Frm" << id()
             << " " << getStructure()
             << " ts = " << std::setprecision(3) << getTimeStamp()
             << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C  " : "");
@@ -353,37 +304,12 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
     {
         _stream << "  <-- ";
         for (auto cby : getConstrainedByList())
-            _stream << "Fac" << cby->id() << " \t";
+            if (cby)
+                _stream << "Fac" << cby->id() << " \t";
     }
     _stream << std::endl;
 
-    if (_metric && _state_blocks)
-    {
-        for (const auto& key : getStructure())
-        {
-            auto sb = getStateBlock(key);
-            _stream << _tabs << "  " << key
-                    << "[" << (sb->isFixed() ? "Fix" : "Est")
-                    << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
-                    << std::endl;
-        }
-    }
-    else if (_metric)
-    {
-        _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est");
-        _stream << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )";
-        _stream << std::endl;
-    }
-    else if (_state_blocks)
-    {
-        _stream << _tabs << "  " << "sb:";
-        for (const auto& key : getStructure())
-        {
-            const auto& sb = getStateBlock(key);
-            _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
-        }
-        _stream << std::endl;
-    }
+    printState(_metric, _state_blocks, _stream, _tabs);
 }
 
 void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -391,7 +317,8 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 2)
         for (auto C : getCaptureList())
-            C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (C)
+                C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
@@ -400,7 +327,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
     std::stringstream inconsistency_explanation;
 
     if (_verbose) {
-        _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "EFrm") : "Frm")
+        _stream << _tabs << "Frm"
                 << id() << " @ " << _frm_ptr.get() << std::endl;
         _stream << _tabs << "  " << "-> Prb @ " << getProblem().get() << std::endl;
         _stream << _tabs << "  " << "-> Trj @ " << getTrajectory().get() << std::endl;
@@ -433,11 +360,6 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
                               << " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
     log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
 
-    // // check trajectory pointer
-    // inconsistency_explanation << "Frame trajectory pointer is " << getTrajectory()
-    //                           << " but trajectory pointer is" << trajectory_ptr << "\n";
-    // log.assertTrue((getTrajectory() == T, inconsistency_explanation);
-
     //  check constrained_by
     for (auto cby : getConstrainedByList())
     {
@@ -473,9 +395,8 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
     inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
                               << " ---> Trj" << " @ " << trj_ptr
                               << " -X-> Frm" << id();
-    auto trj_frm_list = trj_ptr->getFrameList();
-    auto trj_has_frm = std::find_if(trj_frm_list.begin(), trj_frm_list.end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
-    log.assertTrue(trj_has_frm != trj_frm_list.end(), inconsistency_explanation);
+    auto trj_has_frm = std::find_if(trj_ptr->begin(), trj_ptr->end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
+    log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation);
 
     for(auto C : getCaptureList())
     {
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 72f07b8e216e6ca830ec9021dbb7ca1144d09860..e6e5a15c3c216d2abfa9242a48bb97081e22d016 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -30,7 +30,8 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 1)
         for (auto S : getSensorList())
-            S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (S)
+                S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 0fdd18775a2ea50c2ca6ee51ffaeaff9db7c85fa..11e26334cfedde1e6ffcae6f4588a57651c51eaa 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -21,11 +21,11 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
 {
     if (_p_ptr)
     {
-        addStateBlock("P", _p_ptr, nullptr);
+        addStateBlock('P', _p_ptr, nullptr);
     }
     if (_o_ptr)
     {
-        addStateBlock("O", _o_ptr, nullptr);
+        addStateBlock('O', _o_ptr, nullptr);
     }
 
 }
@@ -165,33 +165,12 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
     {
         _stream << "\t<-- ";
         for (auto cby : getConstrainedByList())
-            _stream << "Fac" << cby->id() << " \t";
+            if (cby)
+                _stream << "Fac" << cby->id() << " \t";
     }
     _stream << std::endl;
 
-    if (_metric && _state_blocks){
-        for (const auto& key : getStructure())
-        {
-            auto sb = getStateBlock(key);
-            _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
-        }
-    }
-    else if (_metric)
-    {
-        _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est");
-        _stream << ",\t x = ( " << std::setprecision(2) << getStateVector().transpose() << " )";
-        _stream << std::endl;
-    }
-    else if (_state_blocks)
-    {
-        _stream << _tabs << "  " << "sb:";
-        for (const auto& key : getStructure())
-        {
-            const auto& sb = getStateBlock(key);
-            _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
-        }
-        _stream << std::endl;
-    }
+    printState(_metric, _state_blocks, _stream, _tabs);
 }
 
 void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 67c37971b4e0517017f03b9cd798080fcd40808d..7f6db136f0d1644bb4d2ff193fc3c7d776fdfa26 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -99,7 +99,8 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 1)
         for (auto L : getLandmarkList())
-            L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (L)
+                L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 813077742ceca8ee0b7b60913fab479145c4298b..3dceffdd79180d3bb3bd9f5d4a27d33321ace5b7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -325,20 +325,17 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
     return (*sen_it);
 }
 
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
-                                   const TimeStamp& _time_stamp, //
-                                   const StateStructure& _frame_structure, //
-                                   const SizeEigen _dim, //
-                                   const Eigen::VectorXd& _frame_state)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+                                      const StateStructure& _frame_structure, //
+                                      const SizeEigen _dim, //
+                                      const Eigen::VectorXd& _frame_state)
 {
     VectorComposite state;
     SizeEigen index = 0;
     SizeEigen size = 0;
-    for (const auto& ckey : _frame_structure)
+    for (const auto& key : _frame_structure)
     {
-        const auto& key = string(1,ckey); // ckey is char
-
-        if (key == "O")
+        if (key == 'O')
         {
             if (_dim == 2) size = 1;
             else size = 4;
@@ -350,69 +347,71 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
 
         state.emplace(key, _frame_state.segment(index, size));
 
+        // append new key to frame structure
+        if (frame_structure_.find(key) == std::string::npos) // not found
+            frame_structure_ += std::string(1,key);
+
         index += size;
     }
 
     assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
 
     auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
-                                             _frame_key_type,
                                              _time_stamp,
                                              _frame_structure,
                                              state);
+
     return frm;
 }
 
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
-                                   const TimeStamp& _time_stamp, //
-                                   const StateStructure& _frame_structure, //
-                                   const SizeEigen _dim)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+                                      const StateStructure& _frame_structure, //
+                                      const SizeEigen _dim)
 {
-    return emplaceFrame(_frame_key_type,
-                        _time_stamp,
+    return emplaceFrame(_time_stamp,
                         _frame_structure,
                         getState(_time_stamp));
 }
 
-FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
-                                    const TimeStamp& _time_stamp, //
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
                                     const StateStructure& _frame_structure, //
                                     const VectorComposite& _frame_state)
 {
     return FrameBase::emplace<FrameBase>(getTrajectory(),
-                                         _frame_key_type,
                                          _time_stamp,
                                          _frame_structure,
                                          _frame_state);
 }
 
-FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
-                                    const TimeStamp& _time_stamp, //
-                                    const VectorComposite& _frame_state)
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
+                                       const VectorComposite& _frame_state)
 {
+    // append new keys to frame structure
+    for (const auto& pair_key_vec : _frame_state)
+    {
+        const auto& key = pair_key_vec.first;
+        if (frame_structure_.find(key) == std::string::npos) // not found
+            frame_structure_ += std::string(1,key);
+    }
+
     return FrameBase::emplace<FrameBase>(getTrajectory(),
-                                         _frame_key_type,
                                          _time_stamp,
                                          getFrameStructure(),
                                          _frame_state);
 }
 
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
-                                   const TimeStamp& _time_stamp, //
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
                                    const Eigen::VectorXd& _frame_state)
 {
-    return emplaceFrame(_frame_key_type,
-                        _time_stamp,
+    return emplaceFrame(_time_stamp,
                         this->getFrameStructure(),
                         this->getDim(),
                         _frame_state);
 }
 
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
-                                   const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
 {
-    return emplaceFrame(_frame_key_type,
-                        _time_stamp,
+    return emplaceFrame(_time_stamp,
                         this->getFrameStructure(),
                         this->getDim());
 }
@@ -429,15 +428,14 @@ TimeStamp Problem::getTimeStamp ( ) const
 
     if (not ts.ok())
     {
-        const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+        const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
 
         if (last_kf_or_aux)
             ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
 
     }
 
-    if (not ts.ok())
-        WOLF_WARN( "Problem has nowhere to find a valid timestamp!");
+    WOLF_DEBUG_COND(not ts.ok(), "Problem has nowhere to find a valid timestamp!");
 
     return ts;
 }
@@ -464,7 +462,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
 
     VectorComposite state_last;
 
-    const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+    const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
 
     if (last_kf_or_aux)
         state_last = last_kf_or_aux->getState(structure);
@@ -472,9 +470,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
     else if (prior_options_ and prior_options_->mode != "nothing")
         state_last = prior_options_->state;
 
-    for (const auto& ckey : structure)
+    for (const auto& key : structure)
     {
-        const auto& key = string(1,ckey);
         if (state.count(key) == 0)
         {
             auto state_last_it = state_last.find(key);
@@ -483,7 +480,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
                 state.emplace(key, state_last_it->second);
 
             else
-                state.emplace(key, stateZero(key).at(key));
+                state.emplace(key, stateZero(string(1,key)).at(key));
         }
     }
 
@@ -514,7 +511,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
 
     VectorComposite state_last;
 
-    const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    const auto& last_kf_or_aux = trajectory_ptr_->closestFrameToTimeStamp(_ts);
 
     if (last_kf_or_aux)
         state_last = last_kf_or_aux->getState(structure);
@@ -522,9 +519,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
     else if (prior_options_ and prior_options_->mode != "nothing")
         state_last = prior_options_->state;
 
-    for (const auto& ckey : structure)
+    for (const auto& key : structure)
     {
-        const auto& key = string(1,ckey);
         if (state.count(key) == 0)
         {
             auto state_last_it = state_last.find(key);
@@ -533,7 +529,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
                 state.emplace(key, state_last_it->second);
 
             else
-                state.emplace(key, stateZero(key).at(key));
+                state.emplace(key, stateZero(string(1,key)).at(key));
         }
     }
 
@@ -544,16 +540,17 @@ SizeEigen Problem::getDim() const
 {
     return dim_;
 }
-StateStructure Problem::getFrameStructure() const
+
+const StateStructure& Problem::getFrameStructure() const
 {
     return frame_structure_;
 }
 
 void Problem::appendToStructure(const StateStructure& _structure)
 {
-    for (const auto& ckey : _structure)
-        if (frame_structure_.find(ckey) == std::string::npos) // now key not found in problem structure -> append!
-            frame_structure_ += ckey;
+    for (const auto& key : _structure)
+        if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
+            frame_structure_ += key;
 }
 
 void Problem::setTreeManager(TreeManagerBasePtr _gm)
@@ -589,11 +586,10 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
     StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
 
     VectorComposite state;
-    for (const auto& ckey : structure)
+    for (const auto& key : structure)
     {
-        const auto& key = string(1,ckey);
         VectorXd vec;
-        if (key == "O")
+        if (key == 'O')
         {
             if (dim_ == 2) vec = VectorXd::Zero(1);
             else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
@@ -622,6 +618,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
     WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
     WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
 
+    // pause processor profiling
+    if (_processor_ptr)
+        _processor_ptr->stopCaptureProfiling();
+
     for (auto sensor : hardware_ptr_->getSensorList())
         for (auto processor : sensor->getProcessorList())
             if (processor && (processor != _processor_ptr) )
@@ -646,31 +646,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
     // notify tree manager
     if (tree_manager_)
         tree_manager_->keyFrameCallback(_keyframe_ptr);
-}
-
-bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
-{
-    // This should implement a black list of processors that have forbidden auxiliary frame creation
-    // This decision is made at problem level, not at processor configuration level.
-    // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
 
-    // Currently allowing all processors to vote:
-    return true;
-}
-
-void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
-{
-    // TODO
-//    if (_processor_ptr)
-//    {
-//        WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
-//    }
-//    else
-//    {
-//        WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
-//    }
-//
-//    processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+    // resume processor profiling
+    if (_processor_ptr)
+        _processor_ptr->startCaptureProfiling();
 }
 
 StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
@@ -876,15 +855,9 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
     return success;
 }
 
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
+bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
 {
-    FrameBasePtr frm = getLastKeyFrame();
-    return getFrameCovariance(frm, cov);
-}
-
-bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const
-{
-    FrameBasePtr frm = getLastKeyOrAuxFrame();
+    FrameBasePtr frm = getLastFrame();
     return getFrameCovariance(frm, cov);
 }
 
@@ -968,24 +941,9 @@ FrameBasePtr Problem::getLastFrame() const
     return trajectory_ptr_->getLastFrame();
 }
 
-FrameBasePtr Problem::getLastKeyFrame() const
-{
-    return trajectory_ptr_->getLastKeyFrame();
-}
-
-FrameBasePtr Problem::getLastKeyOrAuxFrame() const
-{
-    return trajectory_ptr_->getLastKeyOrAuxFrame();
-}
-
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
 {
-    return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
-
-FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
-{
-    return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    return trajectory_ptr_->closestFrameToTimeStamp(_ts);
 }
 
 void Problem::setPriorOptions(const std::string& _mode,
@@ -1041,9 +999,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
 
     if (prior_options_->mode != "nothing" and prior_options_->mode != "")
     {
-        prior_keyframe = emplaceFrame(KEY,
-                                      _ts,
-                                      prior_options_->state);
+        prior_keyframe = emplaceFrame(_ts,
+                                         prior_options_->state);
 
         if (prior_options_->mode == "fix")
             prior_keyframe->fix();
@@ -1067,7 +1024,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
                 assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
 
                 // feature
-                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + key, state_blk, covar_blk);
+                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), state_blk, covar_blk);
 
                 // factor
                 if (sb->hasLocalParametrization())
@@ -1203,14 +1160,11 @@ void Problem::perturb(double amplitude)
         S->perturb(amplitude);
 
     // Frames and Captures
-    for (auto& F : getTrajectory()->getFrameList())
+    for (auto& F : *(getTrajectory()))
     {
-        if (F->isKeyOrAux())
-        {
-            F->perturb(amplitude);
-            for (auto& C : F->getCaptureList())
-                C->perturb(amplitude);
-        }
+        F->perturb(amplitude);
+        for (auto& C : F->getCaptureList())
+            C->perturb(amplitude);
     }
 
     // Landmarks
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
index 1bfd06a5e9669f76ef48c49a559cade0dc7845f0..a17142e80bf69198674b29f0dc5f3bde955412a4 100644
--- a/src/processor/is_motion.cpp
+++ b/src/processor/is_motion.cpp
@@ -5,5 +5,6 @@ using namespace wolf;
 
 void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
 {
+    setOdometry(_prb_ptr->stateZero(state_structure_));
     _prb_ptr->addProcessorIsMotion(_motion_ptr);
 }
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index c02228864e1db24fb0b43826736a26c1c6688451..e8f891b8c51098909b5db479f7e3d3a8139457c0 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -12,7 +12,13 @@ ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessor
         processor_id_(++processor_id_count_),
         params_(_params),
         dim_(_dim),
-        sensor_ptr_()
+        sensor_ptr_(),
+        n_capture_callback_(0),
+        n_kf_callback_(0),
+        acc_duration_capture_(0),
+        acc_duration_kf_(0),
+        max_duration_capture_(0),
+        max_duration_kf_(0)
 {
 //    WOLF_DEBUG("constructed    +p" , id());
 }
@@ -27,16 +33,14 @@ bool ProcessorBase::permittedKeyFrame()
     return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
 }
 
-bool ProcessorBase::permittedAuxFrame()
-{
-    return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this());
-}
-
 void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other)
 {
     assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
     WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
 
+    // profiling
+    n_kf_callback_++;
+    startKFProfiling();
 
     // asking if key frame should be stored
     if (storeKeyFrame(_keyframe_ptr))
@@ -46,6 +50,8 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _
     if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
         processKeyFrame(_keyframe_ptr, _time_tol_other);
 
+    // profiling
+    stopKFProfiling();
 }
 
 void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
@@ -53,6 +59,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
     assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
     WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
 
+    // profiling
+    n_capture_callback_++;
+    startCaptureProfiling();
+
     // apply prior in problem if not done (very first capture)
     if (getProblem() && !getProblem()->isPriorSet())
         getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
@@ -64,6 +74,9 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
     // if trigger, process directly without buffering
     if (triggerInCapture(_capture_ptr))
         processCapture(_capture_ptr);
+
+    // profiling
+    stopCaptureProfiling();
 }
 
 void ProcessorBase::remove()
@@ -101,7 +114,7 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
     }
     else
     {
-        WOLF_WARN("Linking with a nullptr");
+        WOLF_WARN("Linking Processor ", id(), " to a nullptr");
     }
 }
 
@@ -118,7 +131,7 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
     // adding processor is motion to the processor is motion vector
     auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
     if (is_motion_ptr)
-        getProblem()->addProcessorIsMotion(is_motion_ptr);
+        is_motion_ptr->addToProblem(_problem, is_motion_ptr);
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////
@@ -220,7 +233,6 @@ void BufferPackKeyFrame::print(void) const
 void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
     _stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
-
 }
 
 void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
@@ -261,4 +273,22 @@ bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, b
     _log.compose(local_log);
     return _log.is_consistent_;
 }
+
+void ProcessorBase::printProfiling(std::ostream& _stream) const
+{
+    _stream << "\n" << getType() << " - " << getName() << ":"
+            << "\n\ttotal time:           " << 1e-6*(acc_duration_capture_ + acc_duration_kf_).count()<< " s"
+            << "\n\tProcessing captures:"
+            << "\n\t\ttotal time:         " << 1e-6*acc_duration_capture_.count() << " s"
+            << "\n\t\tcaptures processed: " << n_capture_callback_
+            << "\n\t\taverage time:       " << 1e-3*acc_duration_capture_.count() / n_capture_callback_ << " ms"
+            << "\n\t\tmax time:           " << 1e-3*max_duration_capture_.count() << " ms"
+            << "\n\tProcessing keyframes:"
+            << "\n\t\ttotal time:         " << 1e-6*acc_duration_kf_.count() << " s"
+            << "\n\t\tkf processed:       " << n_kf_callback_
+            << "\n\t\taverage time:       " << 1e-3*acc_duration_kf_.count() / n_kf_callback_ << " ms"
+            << "\n\t\tmax time:           " << 1e-3*max_duration_kf_.count() << " ms" << std::endl;
+
+}
+
 } // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d47dbbad36a9328df374b4b4d08d453db3eae6e1..64536a9195540dedbdb39405e50a658fec3412c0 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -23,6 +23,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
                                  SizeEigen _calib_size,
                                  ParamsProcessorMotionPtr _params_motion) :
         ProcessorBase(_type, _dim, _params_motion),
+        IsMotion(_state_structure),
         params_motion_(_params_motion),
         processing_step_(RUNNING_WITHOUT_KF),
         x_size_(_state_size),
@@ -33,7 +34,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         origin_ptr_(),
         last_ptr_(),
         incoming_ptr_(),
-        dt_(0.0), 
+        last_frame_ptr_(),
+        dt_(0.0),
         x_(_state_size),
         delta_(_delta_size),
         delta_cov_(_delta_cov_size, _delta_cov_size),
@@ -44,7 +46,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
         jacobian_calib_(delta_cov_size_, calib_size_)
 {   
-    setStateStructure(_state_structure);
     //
 }
 
@@ -53,10 +54,45 @@ ProcessorMotion::~ProcessorMotion()
 //    std::cout << "destructed     -p-Mot" << id() << std::endl;
 }
 
-void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
+
+void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+                                    CaptureMotionPtr cap_target)
+{
+    assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions");
+
+    // add prev buffer (discarding the first zero motion)
+    cap_target->getBuffer().pop_front();
+    cap_target->getBuffer().insert(cap_target->getBuffer().begin(),
+                                   cap_prev->getBuffer().begin(),
+                                   cap_prev->getBuffer().end());
+
+    // change origin
+    cap_target->setOriginCapture(cap_prev->getOriginCapture());
+
+    // change calibration params for preintegration from origin
+    cap_target->setCalibrationPreint(getCalibration(cap_prev->getOriginCapture()));
+
+    // reintegrate buffer
+    reintegrateBuffer(cap_target);
+
+    // replace existing feature and factor (if they exist)
+    if (!cap_target->getFeatureList().empty())
+    {
+        // remove feature and factor (automatically)
+        cap_target->getFeatureList().back()->remove();
+
+        assert(cap_target->getFeatureList().empty());// there should be one feature only
+
+        // emplace new feature and factor
+        auto new_feature = emplaceFeature(cap_target);
+        emplaceFactor(new_feature, cap_prev->getOriginCapture());
+    }
+}
+
+void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
                                   TimeStamp _ts_split,
                                   const FrameBasePtr& _keyframe_target,
-                                  const wolf::CaptureMotionPtr& _capture_target)
+                                  const CaptureMotionPtr& _capture_target) const
 {
     /** we are doing this:
      *
@@ -400,10 +436,12 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
          *
          */
 
+        // Set the capture's calibration with best results available (in case origin_ptr_ received a solve())
+        setCalibration(last_ptr_, getCalibration(origin_ptr_));
 
         // Set the frame of last_ptr as key
         auto key_frame      = last_ptr_->getFrame();
-        key_frame           ->setKey();
+        key_frame           ->link(getProblem());
 
         // create motion feature and add it to the key_capture
         auto key_feature    = emplaceFeature(last_ptr_);
@@ -412,18 +450,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         auto factor         = emplaceFactor(key_feature, origin_ptr_);
 
         // create a new frame
-        auto frame_new      = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                         getTimeStamp(),
-                                                         getStateStructure(),
-                                                         getProblem()->getState());
+        auto frame_new      = std::make_shared<FrameBase>(getTimeStamp(),
+                                                          getStateStructure(),
+                                                          getProblem()->getState());
         // create a new capture
         auto capture_new    = emplaceCapture(frame_new,
                                              getSensor(),
                                              key_frame->getTimeStamp(),
                                              Eigen::VectorXd::Zero(data_size_),
                                              getSensor()->getNoiseCov(),
-                                             getCalibration(last_ptr_),
-                                             getCalibration(last_ptr_),
+                                             getCalibration(origin_ptr_),
+                                             getCalibration(origin_ptr_),
                                              last_ptr_);
         // reset the new buffer
         capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
@@ -434,15 +471,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         // Reset pointers
         origin_ptr_     = last_ptr_;
         last_ptr_       = capture_new;
+        last_frame_ptr_ = frame_new;
 
         // callback to other processors
         getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
+
     }
 
     // clear incoming just in case
     incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
 
     postProcess();
+
 }
 
 VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const
@@ -640,10 +680,10 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
 
 FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
 {
-    FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY,
-                                                            _ts_origin,
-                                                            getStateStructure(),
-                                                            _x_origin);
+    FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+                                                               _ts_origin,
+                                                               getStateStructure(),
+                                                               _x_origin);
     setOrigin(key_frame_ptr);
 
     return key_frame_ptr;
@@ -654,7 +694,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
     assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
     assert(_origin_frame->getTrajectory() != nullptr
             && "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
-    assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
 
     TimeStamp origin_ts = _origin_frame->getTimeStamp();
 
@@ -665,25 +704,24 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
                                  origin_ts,
                                  Eigen::VectorXd::Zero(data_size_),
                                  getSensor()->getNoiseCov(),
-                                 getSensor()->getCalibration(),
-                                 getSensor()->getCalibration(),
+                                 getCalibration(),
+                                 getCalibration(),
                                  nullptr);
 
     // ---------- LAST ----------
     // Make non-key-frame for last Capture
-    auto new_frame_ptr  = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                     origin_ts,
-                                                     getStateStructure(),
-                                                     _origin_frame->getState());
+    last_frame_ptr_  = std::make_shared<FrameBase>(origin_ts,
+                                                   getStateStructure(),
+                                                   _origin_frame->getState());
                                         
     // emplace (emtpy) last Capture
-    last_ptr_ = emplaceCapture(new_frame_ptr,
+    last_ptr_ = emplaceCapture(last_frame_ptr_,
                                getSensor(),
                                origin_ts,
                                Eigen::VectorXd::Zero(data_size_),
                                getSensor()->getNoiseCov(),
-                               getSensor()->getCalibration(),
-                               getSensor()->getCalibration(),
+                               getCalibration(),
+                               getCalibration(),
                                origin_ptr_);
 
     // clear and reset buffer
@@ -743,9 +781,12 @@ void ProcessorMotion::integrateOneStep()
                              jacobian_delta_,
                              jacobian_delta_preint_,
                              jacobian_calib_);
+
+    // integrate odometry
+    statePlusDelta(odometry_, delta_, dt_, odometry_);
 }
 
-void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
+void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
 {
     VectorXd calib              = _capture_ptr->getCalibrationPreint();
 
@@ -808,6 +849,19 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
 CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
 {
     assert(_ts.ok());
+    // assert(last_ptr_ != nullptr);
+
+    //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
+    if(last_ptr_ == nullptr) return nullptr;
+
+    // First check if last_ptr is the one we are looking for
+    if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
+        return last_ptr_;
+
+
+    // Then look in the Wolf tree...
+    // -----------------------------
+    //
 
     // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
     // Note: since the buffer goes from a KF in the past until the next KF, we need to:
@@ -817,8 +871,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
     FrameBasePtr     frame          = nullptr;
     CaptureBasePtr   capture        = nullptr;
     CaptureMotionPtr capture_motion = nullptr;
-    for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
-            frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
+    for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
+            frame_rev_iter != getProblem()->getTrajectory()->rend();
             ++frame_rev_iter)
     {
         frame   = *frame_rev_iter;
@@ -923,7 +977,7 @@ TimeStamp ProcessorMotion::getTimeStamp ( ) const
         not last_ptr_)
     {
         WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0");
-        return TimeStamp(0);
+        return TimeStamp::Invalid();
     }
 
     if (getBuffer().empty())
@@ -936,10 +990,10 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
 {
     _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
     if (getOrigin())
-        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? "  KFrm" : "  AFrm" ) : "  Frm")
+        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << "  Frm"
                 << getOrigin()->getFrame()->id() << std::endl;
     if (getLast())
-        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
+        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << " Frm"
                 << getLast()->getFrame()->id() << std::endl;
     if (getIncoming())
         _stream << _tabs << "  " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 284090e238f3c75327424cb76f3946e277e23bdf..2da7c330b59ffce0c7eceb11515e4f992a9eb3d9 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -93,8 +93,8 @@ void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
     assert(_delta.size() == delta_size_ && "Wrong _x_plus_delta vector size");
 
     // This is just a frame composition in 2d
-    _x_plus_delta["P"] = _x.at("P")  + Eigen::Rotation2Dd(_x.at("O")(0)).matrix() * _delta.head<2>();
-    _x_plus_delta["O"] = Vector1d(pi2pi(_x.at("O")(0) + _delta(2)));
+    _x_plus_delta['P'] = _x.at('P')  + Eigen::Rotation2Dd(_x.at('O')(0)).matrix() * _delta.head<2>();
+    _x_plus_delta['O'] = Vector1d(pi2pi(_x.at('O')(0) + _delta(2)));
 }
 
 bool ProcessorOdom2d::voteForKeyFrame() const
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 5b01e98b3ad8fbbe6f31bedc4f25b1a9cfe76747..c7be4d48c4d086d5d4f594dfa7849419d8cc5c30 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -13,7 +13,11 @@ ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
                         min_disp_var_   (0.1),          // around 10cm error
                         min_rot_var_    (0.1)           // around 6 degrees error
 {
-    //
+     // Set constant parts of Jacobians
+    jacobian_delta_preint_.setIdentity(6,6);
+    jacobian_delta_.setIdentity(6,6);
+    jacobian_calib_.setZero(6,0);
+    unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity();   
 }
 
 ProcessorOdom3d::~ProcessorOdom3d()
@@ -147,13 +151,13 @@ void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
 {   
     assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
 
-    Eigen::Map<const Eigen::Vector3d>       p      (_x.at("P").data()     );
-    Eigen::Map<const Eigen::Quaterniond>    q      (_x.at("O").data()     );
+    Eigen::Map<const Eigen::Vector3d>       p      (_x.at('P').data()     );
+    Eigen::Map<const Eigen::Quaterniond>    q      (_x.at('O').data()     );
     Eigen::Map<const Eigen::Vector3d>       dp     (_delta.data()         );
     Eigen::Map<const Eigen::Quaterniond>    dq     (_delta.data()     + 3 );
 
-    _x_plus_delta["P"] = p + q * dp;
-    _x_plus_delta["O"] =    (q * dq).coeffs();
+    _x_plus_delta['P'] = p + q * dp;
+    _x_plus_delta['O'] =    (q * dq).coeffs();
 }
 
 
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index ef801eb6d525375272fd95d89a50eec9bed4dc1e..52421e86398bc049321e8b3dc5f75dc56d2d3001 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -24,6 +24,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
         origin_ptr_(nullptr),
         last_ptr_(nullptr),
         incoming_ptr_(nullptr),
+        last_frame_ptr_(nullptr),
         state_structure_(_structure)
 {
     //
@@ -80,10 +81,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         {
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
 
-            FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY,
-                                                           incoming_ptr_->getTimeStamp(),
-                                                           getStateStructure(),
-                                                           getProblem()->getState(getStateStructure()));
+            FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+                                                              incoming_ptr_->getTimeStamp(),
+                                                              getProblem()->getFrameStructure(),
+                                                              getProblem()->getState());
             incoming_ptr_->link(kfrm);
 
             // Process info
@@ -113,10 +114,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         {
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
 
-            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                          incoming_ptr_->getTimeStamp());
+            FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+                                                           getProblem()->getFrameStructure(),
+                                                           getProblem()->getState());
             incoming_ptr_->link(frm);
-
             // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
 
             // Process info
@@ -130,6 +131,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             resetDerived();
             origin_ptr_ = last_ptr_;
             last_ptr_   = incoming_ptr_;
+            last_frame_ptr_ = frm;
             incoming_ptr_ = nullptr;
 
             break;
@@ -149,8 +151,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             last_old_frame->remove();
 
             // Create new frame
-            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                          incoming_ptr_->getTimeStamp());
+            FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+                                                           getProblem()->getFrameStructure(),
+                                                           getProblem()->getState());
             incoming_ptr_->link(frm);
 
             // Detect new Features, initialize Landmarks, create Factors, ...
@@ -163,6 +166,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             resetDerived();
             origin_ptr_ = last_ptr_;
             last_ptr_   = incoming_ptr_;
+            last_frame_ptr_ = frm;
             incoming_ptr_ = nullptr;
 
             break;
@@ -183,10 +187,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // We create a KF
                 // set KF on last
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-                last_ptr_->getFrame()->setKey();
+                last_ptr_->getFrame()->link(getProblem());
 
                 // // make F; append incoming to new F
-                // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+                // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
                 // incoming_ptr_->link(frm);
 
                 // Establish factors
@@ -199,47 +203,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 resetDerived();
 
                 // make F; append incoming to new F
-                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                              incoming_ptr_->getTimeStamp(),
-                                                              last_ptr_->getFrame()->getState());
+                FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+                                                               getProblem()->getFrameStructure(),
+                                                               last_ptr_->getFrame()->getState());
                 incoming_ptr_->link(frm);
-
                 origin_ptr_ = last_ptr_;
                 last_ptr_   = incoming_ptr_;
+                last_frame_ptr_ = frm;
                 incoming_ptr_ = nullptr;
 
             }
-            /* TODO: create an auxiliary frame
-            else if (voteForAuxFrame() && permittedAuxFrame())
-            {
-                // We create an Auxiliary Frame
-
-                // set AuxF on last
-                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-                last_ptr_->getFrame()->setAuxiliary();
-
-                // make F; append incoming to new F
-                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
-                incoming_ptr_->link(frm);
-
-                // Establish factors
-                establishFactors();
-
-                // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors
-                getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
-
-                // Advance this
-                advanceDerived();
-
-                // Replace last frame for a new one in incoming
-                last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
-                // do not remove! last_ptr_->remove();
-                incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
-
-                // Update pointers
-                last_ptr_   = incoming_ptr_;
-                incoming_ptr_ = nullptr;
-            }*/
             else
             {
                 // We do not create a KF
@@ -248,14 +221,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 advanceDerived();
 
                 // Replace last frame for a new one in incoming
-                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                              incoming_ptr_->getTimeStamp(),
-                                                              last_ptr_->getFrame()->getState());
+                FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+                                                               getProblem()->getFrameStructure(),
+                                                               last_ptr_->getFrame()->getState());
                 incoming_ptr_->link(frm);
                 last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
 
                 // Update pointers
                 last_ptr_       = incoming_ptr_;
+                last_frame_ptr_ = frm;
                 incoming_ptr_   = nullptr;
             }
             break;
@@ -302,11 +276,11 @@ void ProcessorTracker::computeProcessingStep()
 
             if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
             {
-                if (last_ptr_->getFrame()->isKey())
+                if (last_ptr_->getFrame()->getProblem())
                 {
                     WOLF_WARN("||*||");
                     WOLF_INFO(" ... It seems you missed something!");
-                    WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
+                    WOLF_INFO("Pack's KF and last's Frame have matching time stamps (i.e. below time tolerances)");
                     WOLF_INFO("Check the following for correctness:");
                     WOLF_INFO("  - You have all processors installed before starting receiving any data");
                     WOLF_INFO("  - You have configured all your processors with compatible time tolerances");
@@ -337,10 +311,10 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
 {
     _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
     if (getOrigin())
-        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
+        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
                 << getOrigin()->getFrame()->id() << std::endl;
     if (getLast())
-        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
+        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << " Frm"
                 << getLast()->getFrame()->id() << std::endl;
     if (getIncoming())
         _stream << _tabs << "  " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 3804962f62fcbfba13b56e7c64866be7497eeaa8..cda95e5ed6259e7ef11d94d01c3fe939ffe61941 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
 
     if (known_features_last_.empty())
     {
-        WOLF_TRACE("Empty last feature list, returning...");
+        WOLF_DEBUG("Empty last feature list, returning...");
         return 0;
     }
 
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 6d7dd0f50da30dc79e3c5c9cca980bc3364a0349..da6a88b75e98e0ada648f76155f5d12501c21c4d 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -214,7 +214,7 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
         {
             auto& ts  = pair_ts_ftr.first;
             auto& ftr = pair_ts_ftr.second;
-            if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey())
+            if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
                 track_kf[ts] = ftr;
         }
         return track_kf;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index f8c3c76c7e5865d241846516ce9989226d9985cd..0d2d54638d145f333efa0e349990beda54d1b17e 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -20,10 +20,10 @@ SensorBase::SensorBase(const std::string& _type,
         NodeBase("SENSOR", _type),
         HasStateBlocks(""),
         hardware_ptr_(),
-        calib_size_(0),
         sensor_id_(++sensor_id_count_), // simple ID factory
         noise_std_(_noise_size),
-        noise_cov_(_noise_size, _noise_size)
+        noise_cov_(_noise_size, _noise_size),
+        last_capture_(nullptr)
 {
     assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
     assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
@@ -33,15 +33,14 @@ SensorBase::SensorBase(const std::string& _type,
     noise_cov_.setZero();
 
     if (_p_ptr)
-        addStateBlock("P", _p_ptr, _p_dyn);
+        addStateBlock('P', _p_ptr, _p_dyn);
 
     if (_o_ptr)
-        addStateBlock("O", _o_ptr, _o_dyn);
+        addStateBlock('O', _o_ptr, _o_dyn);
 
     if (_intr_ptr)
-        addStateBlock("I", _intr_ptr, _intr_dyn);
+        addStateBlock('I', _intr_ptr, _intr_dyn);
 
-    updateCalibSize();
 }
 
 SensorBase::SensorBase(const std::string& _type,
@@ -55,23 +54,21 @@ SensorBase::SensorBase(const std::string& _type,
         NodeBase("SENSOR", _type),
         HasStateBlocks(""),
         hardware_ptr_(),
-        calib_size_(0),
         sensor_id_(++sensor_id_count_), // simple ID factory
         noise_std_(_noise_std),
-        noise_cov_(_noise_std.size(), _noise_std.size())
+        noise_cov_(_noise_std.size(), _noise_std.size()),
+        last_capture_(nullptr)
 {
     setNoiseStd(_noise_std);
 
     if (_p_ptr)
-        addStateBlock("P", _p_ptr, _p_dyn);
+        addStateBlock('P', _p_ptr, _p_dyn);
 
     if (_o_ptr)
-        addStateBlock("O", _o_ptr, _o_dyn);
+        addStateBlock('O', _o_ptr, _o_dyn);
 
     if (_intr_ptr)
-        addStateBlock("I", _intr_ptr, _intr_dyn);
-
-    updateCalibSize();
+        addStateBlock('I', _intr_ptr, _intr_dyn);
 }
 
 SensorBase::~SensorBase()
@@ -82,9 +79,8 @@ SensorBase::~SensorBase()
 
 void SensorBase::removeStateBlocks()
 {
-    for (const auto& _key : getStructure())
+    for (const auto& key : getStructure())
     {
-        auto key = std::string(1,_key);
         auto sbp = getStateBlock(key);
         if (sbp != nullptr)
         {
@@ -105,7 +101,6 @@ void SensorBase::fixExtrinsics()
         if (sbp != nullptr)
             sbp->fix();
     }
-    updateCalibSize();
 }
 
 void SensorBase::unfixExtrinsics()
@@ -116,40 +111,35 @@ void SensorBase::unfixExtrinsics()
         if (sbp != nullptr)
             sbp->unfix();
     }
-    updateCalibSize();
 }
 
 void SensorBase::fixIntrinsics()
 {
-    for (const auto& _key : getStructure())
+    for (const auto& key : getStructure())
     {
-        auto key = std::string(1,_key);
-        if (key != "P" and key != "O") // exclude extrinsics
+        if (key != 'P' and key != 'O') // exclude extrinsics
         {
             auto sbp = getStateBlockDynamic(key);
             if (sbp != nullptr)
                 sbp->fix();
         }
     }
-    updateCalibSize();
 }
 
 void SensorBase::unfixIntrinsics()
 {
-    for (const auto& _key : getStructure())
+    for (const auto& key : getStructure())
     {
-        auto key = std::string(1,_key);
-        if (key != "P" and key != "O") // exclude extrinsics
+        if (key != 'P' and key != 'O') // exclude extrinsics
         {
             auto sbp = getStateBlockDynamic(key);
             if (sbp != nullptr)
                 sbp->unfix();
         }
     }
-    updateCalibSize();
 }
 
-void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
 {
     assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
 
@@ -201,7 +191,7 @@ void SensorBase::registerNewStateBlocks() const
             auto key = pair_key_sbp.first;
             auto sbp = pair_key_sbp.second;
 
-            if (sbp && !isStateBlockInCapture(key))
+            if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key))
                 getProblem()->notifyStateBlock(sbp, ADD);
         }
     }
@@ -217,131 +207,94 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
     noise_cov_ = _noise_cov;
 }
 
-CaptureBasePtr SensorBase::lastCapture(void) const
+void SensorBase::setLastCapture(CaptureBasePtr cap)
 {
-    // we search for the most recent Capture of this sensor which belongs to a KeyFrame
-    CaptureBasePtr capture = nullptr;
-    if (getProblem())
-    {
-        FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-        FrameBaseRevIter frame_rev_it = frame_list.rbegin();
-        while (frame_rev_it != frame_list.rend())
-        {
-            capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
-            if (capture)
-                // found the most recent Capture made by this sensor !
-                break;
-            frame_rev_it++;
-        }
-    }
-    return capture;
+    assert(cap);
+    assert(cap->getSensor() == shared_from_this());
+    assert(cap->getTimeStamp().ok());
+    assert(not last_capture_ or last_capture_->getTimeStamp() < cap->getTimeStamp());
+    last_capture_ = cap;
 }
 
-CaptureBasePtr SensorBase::lastKeyCapture(void) const
+void SensorBase::updateLastCapture()
 {
     // we search for the most recent Capture of this sensor which belongs to a KeyFrame
-    CaptureBasePtr capture = nullptr;
     if (getProblem())
     {
-        FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-        FrameBaseRevIter frame_rev_it = frame_list.rbegin();
-        while (frame_rev_it != frame_list.rend())
+        // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+        auto trajectory = getProblem()->getTrajectory();
+        TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+        while (frame_rev_it != trajectory->rend())
         {
-            if ((*frame_rev_it)->isKey())
+            auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+            if (capture and not capture->isRemoving())
             {
-                capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
-                if (capture)
-                    // found the most recent Capture made by this sensor !
-                    break;
+                // found the most recent Capture made by this sensor !
+                last_capture_ = capture;
+                return;
             }
             frame_rev_it++;
         }
     }
-    return capture;
+
+    // no captures found
+    last_capture_ = nullptr;
 }
 
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
 {
     // we search for the most recent Capture of this sensor before _ts
-    CaptureBasePtr capture = nullptr;
-    if (getProblem())
-    {
-        FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+    if (not getProblem())
+        return nullptr;
 
-        // We iterate in reverse since we're likely to find it close to the rbegin() place.
-        FrameBaseRevIter frame_rev_it = frame_list.rbegin();
-        while (frame_rev_it != frame_list.rend())
+    // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+    auto trajectory = getProblem()->getTrajectory();
+
+    // We iterate in reverse since we're likely to find it close to the rbegin() place.
+    TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+    while (frame_rev_it != trajectory->rend())
+    {
+        if ((*frame_rev_it)->getTimeStamp() <= _ts)
         {
-            if ((*frame_rev_it)->getTimeStamp() <= _ts)
-            {
-                capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
-                if (capture)
-                    // found the most recent Capture made by this sensor !
-                    break;
-            }
-            frame_rev_it++;
+            auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+            if (capture)
+                // found the most recent Capture made by this sensor !
+                return capture;
         }
+        frame_rev_it++;
     }
-    return capture;
+
+    return nullptr;
 }
 
 StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
 {
-    return getStateBlockDynamic("P", _ts);
+    return getStateBlockDynamic('P', _ts);
 }
 
 StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
 {
-    return getStateBlockDynamic("O", _ts);
+    return getStateBlockDynamic('O', _ts);
 }
 
 StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
 {
-    return getStateBlockDynamic("I", _ts);
+    return getStateBlockDynamic('I', _ts);
 }
 
 StateBlockPtr SensorBase::getP() const
 {
-    return getStateBlockDynamic("P");
+    return getStateBlockDynamic('P');
 }
 
 StateBlockPtr SensorBase::getO() const
 {
-    return getStateBlockDynamic("O");
+    return getStateBlockDynamic('O');
 }
 
 StateBlockPtr SensorBase::getIntrinsic() const
 {
-    return getStateBlockDynamic("I");
-}
-
-SizeEigen SensorBase::computeCalibSize() const
-{
-    SizeEigen sz = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-    {
-        auto sb = pair_key_sb.second;
-        if (sb && !sb->isFixed())
-            sz += sb->getSize();
-    }
-    return sz;
-}
-
-Eigen::VectorXd SensorBase::getCalibration() const
-{
-    SizeEigen index = 0;
-    SizeEigen sz = getCalibSize();
-    Eigen::VectorXd calib(sz);
-    for (const auto& key : getStructure())
-    {
-        auto sb = getStateBlockDynamic(key);
-        if (sb && !sb->isFixed())
-        {
-            calib.segment(index, sb->getSize()) = sb->getState();
-            index += sb->getSize();
-        }
-    }
-    return calib;
+    return getStateBlockDynamic('I');
 }
 
 bool SensorBase::process(const CaptureBasePtr capture_ptr)
@@ -382,7 +335,7 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
     processor_list_.remove(_proc_ptr);
 }
 
-StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key) const
 {
     CaptureBasePtr cap;
 
@@ -392,7 +345,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key) const
     return getStateBlock(_key);
 }
 
-StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const
+StateBlockPtr SensorBase::getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const
 {
     CaptureBasePtr cap;
 
@@ -402,12 +355,11 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const Ti
     return getStateBlock(_key);
 }
 
-bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) const
 {
     if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
     {
-        _cap = lastCapture();
-        //        cap = lastKeyCapture();
+        _cap = last_capture_;
 
         return _cap != nullptr;
     }
@@ -415,11 +367,11 @@ bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr&
         return false;
 }
 
-bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const
 {
     if (isStateBlockDynamic(_key))
     {
-        _cap = lastCapture(_ts);
+        _cap = findLastCaptureBefore(_ts);
 
         return _cap != nullptr;
     }
@@ -427,14 +379,14 @@ bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp&
         return false;
 }
 
-bool SensorBase::isStateBlockInCapture(const std::string& _key) const
+bool SensorBase::isStateBlockInCapture(const char& _key) const
 {
     CaptureBasePtr cap;
 
     return isStateBlockInCapture(_key, cap);
 }
 
-bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const
+bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const
 {
     CaptureBasePtr cap;
 
@@ -463,7 +415,7 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
     }
     else
     {
-        WOLF_WARN("Linking with a nullptr");
+        WOLF_WARN("Linking Sensor ", id(), " to a nullptr");
     }
 }
 
@@ -474,39 +426,7 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
         _stream << " -- " << getProcessorList().size() << "p";
     _stream << std::endl;
 
-    if (_metric && _state_blocks)
-    {
-        _stream << _tabs << "  " << "sb: ";
-        for (auto& _key : getStructure())
-        {
-            auto key = std::string(1,_key);
-            auto sb = getStateBlock(key);
-            _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
-        }
-        _stream << std::endl;
-    }
-    else if (_metric)
-    {
-        _stream << _tabs << "  " << "( ";
-        for (auto& _key : getStructure())
-        {
-            auto key = std::string(1,_key);
-            auto sb = getStateBlock(key);
-            _stream << sb->getState().transpose() << " ";
-        }
-        _stream << ")" << std::endl;
-    }
-    else if (_state_blocks)
-    {
-        _stream << _tabs << "  " << "sb: ";
-        for (auto& _key : getStructure())
-        {
-            auto key = std::string(1,_key);
-            auto sb = getStateBlock(key);
-            _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
-        }
-        _stream << std::endl;
-    }
+    printState(_metric, _state_blocks, _stream, _tabs);
 }
 
 void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -515,7 +435,8 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
 
     if (_depth >= 2)
         for (auto p : getProcessorList())
-            p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (p)
+                p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
@@ -561,6 +482,20 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
                                       << " -X-> Sen" << id();
             log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
         }
+        // check last_capture_
+        if (getProblem()->getTimeStamp().ok())
+        {
+            auto last_capture_found = findLastCaptureBefore(getProblem()->getTimeStamp());
+            inconsistency_explanation << "SensorBase::last_capture_: "
+                                      << (last_capture_ ? std::to_string(last_capture_->id()) : "-")
+                                      << " @ " << last_capture_
+                                      << " is not the actual last capture: "
+                                      << (last_capture_found ?
+                                          std::to_string(last_capture_found->id()) :
+                                          "-")
+                                      << " @ " << last_capture_found << std::endl;
+            log.assertTrue(last_capture_ == last_capture_found, inconsistency_explanation);
+        }
         return log;
 }
 
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 24a674f5f517e7bbd5ac8448c8678a7517632b90..99dfb8729c9bc5cb05f00758494571de9d52fc23 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -5,10 +5,29 @@
 
 namespace wolf {
 
-SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
-          wolf_problem_(_wolf_problem)
+SolverManager::SolverManager(const ProblemPtr& _problem) :
+        SolverManager(_problem, std::make_shared<ParamsSolver>())
 {
-    assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
+}
+
+SolverManager::SolverManager(const ProblemPtr& _problem,
+                             const ParamsSolverPtr& _params) :
+          n_solve_(0),
+          n_cov_(0),
+          acc_duration_total_(0),
+          acc_duration_solver_(0),
+          acc_duration_update_(0),
+          acc_duration_state_(0),
+          acc_duration_cov_(0),
+          max_duration_total_(0),
+          max_duration_solver_(0),
+          max_duration_update_(0),
+          max_duration_state_(0),
+          max_duration_cov_(0),
+          wolf_problem_(_problem),
+          params_(_params)
+{
+    assert(_problem != nullptr && "Passed a nullptr ProblemPtr.");
 }
 
 SolverManager::~SolverManager()
@@ -68,10 +87,19 @@ void SolverManager::update()
     }
 
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
+    std::set<StateBlockPtr> new_floating_state_blocks;
     for (auto state_pair : state_blocks_)
     {
         auto state_ptr = state_pair.first;
 
+        // Check for "floating" state blocks (not involved in any factor -> not observable problem)
+        if (state_blocks_2_factors_.at(state_ptr).empty())
+        {
+            WOLF_DEBUG("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart.");
+            new_floating_state_blocks.insert(state_ptr);
+            continue;
+        }
+
         // state update
         if (state_ptr->stateUpdated())
             updateStateBlockState(state_ptr);
@@ -85,6 +113,20 @@ void SolverManager::update()
             updateStateBlockLocalParametrization(state_ptr);
     }
 
+    // REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call)
+    for (auto state_ptr : new_floating_state_blocks)
+    {
+        removeStateBlock(state_ptr);
+        floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()!
+    }
+    // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock)
+    for (auto state_ptr : floating_state_blocks_)
+    {
+        state_ptr->resetStateUpdated();
+        state_ptr->resetFixUpdated();
+        state_ptr->resetLocalParamUpdated();
+    }
+
     #ifdef _WOLF_DEBUG
         assert(check("after update()"));
     #endif
@@ -95,14 +137,32 @@ wolf::ProblemPtr SolverManager::getProblem()
     return wolf_problem_;
 }
 
+std::string SolverManager::solve()
+{
+    return solve(params_->verbose);
+}
+
 std::string SolverManager::solve(const ReportVerbosity report_level)
 {
+    auto start_total = std::chrono::high_resolution_clock::now();
+    n_solve_++;
+
     // update problem
+    auto start_update = std::chrono::high_resolution_clock::now();
     update();
+    auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update);
+    acc_duration_update_ += duration_update;
+    max_duration_update_ = std::max(max_duration_update_,duration_update);
 
+    // call derived solver
+    auto start_solver = std::chrono::high_resolution_clock::now();
     std::string report = solveDerived(report_level);
+    auto duration_solver = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_solver);
+    acc_duration_solver_ += duration_solver;
+    max_duration_solver_ = std::max(max_duration_solver_,duration_solver);
 
     // update StateBlocks with optimized state value.
+    auto start_state = std::chrono::high_resolution_clock::now();
     /// @todo whatif someone has changed the state notification during opti ??
     /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
 
@@ -114,16 +174,52 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
         if (!stateblock_statevector.first->isFixed())
             stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
     }
+    auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
+    acc_duration_state_ += duration_state;
+    max_duration_state_ = std::max(max_duration_state_,duration_state);
+
+    auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total);
+    acc_duration_total_ += duration_total;
+    max_duration_total_ = std::max(max_duration_total_,duration_total);
 
     return report;
 }
 
+
+bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
+{
+    auto start_cov = std::chrono::high_resolution_clock::now();
+    n_cov_++;
+
+    auto ret = computeCovariancesDerived(blocks);
+
+    auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+    acc_duration_cov_ += duration_cov;
+    max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+    return ret;
+}
+
+bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+{
+    auto start_cov = std::chrono::high_resolution_clock::now();
+    n_cov_++;
+
+    auto ret = computeCovariancesDerived(st_list);
+
+    auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+    acc_duration_cov_ += duration_cov;
+    max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+    return ret;
+}
+
 void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
 {
     // Warning if adding an already added
     if (factors_.count(fac_ptr) != 0)
     {
-        WOLF_WARN("Tried to add a factor that was already added !");
+        WOLF_WARN("Tried to add the factor ", fac_ptr->id(), " that was already added !");
         return;
     }
 
@@ -137,10 +233,20 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
     for (const auto& st : fac_ptr->getStateBlockPtrVector())
         if (state_blocks_.count(st) == 0)
         {
-            WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
-            // put back notification in problem (will be added next update() call) and do nothing
-            wolf_problem_->notifyFactor(fac_ptr, ADD);
-            return;
+            // Check if it was stored as a 'floating' state block
+            if (floating_state_blocks_.count(st) == 1)
+            {
+                WOLF_DEBUG("SolverManager::addFactor(): Factor ", fac_ptr->id(), " involves state block ", st, " stored as 'floating'. Adding the state block to the solver.");
+                floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()!
+                addStateBlock(st);
+            }
+            else
+            {
+                WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+                // put back notification in problem (will be added next update() call) and do nothing
+                wolf_problem_->notifyFactor(fac_ptr, ADD);
+                return;
+            }
         }
 
     // factors
@@ -163,7 +269,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
     // Warning if removing a missing factor
     if (factors_.count(fac_ptr) == 0)
     {
-        WOLF_WARN("Tried to remove a factor that is missing !");
+        WOLF_WARN("Tried to remove factor ", fac_ptr->id(), " that is missing !");
         return;
     }
 
@@ -187,12 +293,19 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
     // Warning if adding an already added state block
     if (state_blocks_.count(state_ptr) != 0)
     {
-        WOLF_WARN("Tried to add a StateBlock that was already added !");
+        WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that was already added !");
+        return;
+    }
+    // Warning if adding a floating state block
+    if (floating_state_blocks_.count(state_ptr) != 0)
+    {
+        WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that is stored as floating !");
         return;
     }
 
     assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
     assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+    assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)");
 
     // stateblock maps
     state_blocks_.emplace(state_ptr, state_ptr->getState());
@@ -209,10 +322,16 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
 
 void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
 {
+    // check if stored as 'floating' state block
+    if (floating_state_blocks_.count(state_ptr) == 1)
+    {
+        floating_state_blocks_.erase(state_ptr);
+        return;
+    }
     // Warning if removing a missing state block
     if (state_blocks_.count(state_ptr) == 0)
     {
-        WOLF_WARN("Tried to remove a StateBlock that was not added !");
+        WOLF_WARN("Tried to remove the StateBlock ", state_ptr, " that was not added !");
         return;
     }
 
@@ -257,8 +376,12 @@ void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
 
 void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
 {
+    assert(state_ptr && "SolverManager::updateStateBlockState null state block");
+    assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block");
+    assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
+    assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size());
+
     Eigen::VectorXd new_state = state_ptr->getState();
-    // We assume the same size for the states in both WOLF and the solver.
     std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
     // reset flag
     state_ptr->resetStateUpdated();
@@ -278,8 +401,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
     auto it = state_blocks_.find(state_ptr);
 
     if (it == state_blocks_.end())
+    {
+        WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
         throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+    }
     return it->second;
 }
 
@@ -288,8 +413,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state
     auto it = state_blocks_.find(state_ptr);
 
     if (it == state_blocks_.end())
-        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+    {
+        WOLF_ERROR("Tried to retrieve the memory block const ptr of an unregistered StateBlock: ", state_ptr);
+        throw std::runtime_error("Tried to retrieve the memory block const ptr of an unregistered StateBlock !");
+    }
     return it->second.data();
 }
 
@@ -298,21 +425,85 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
     auto it = state_blocks_.find(state_ptr);
 
     if (it == state_blocks_.end())
-        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+    {
+        WOLF_ERROR("Tried to retrieve the memory block ptr of an unregistered StateBlock: ", state_ptr);
+        throw std::runtime_error("Tried to retrieve the memory block ptr of an unregistered StateBlock !");
+    }
     return it->second.data();
 }
 
-bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
+SolverManager::ReportVerbosity SolverManager::getVerbosity() const
 {
-    return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);
+    return params_->verbose;
+}
+
+bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
+{
+    return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+}
+
+bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
+{
+    return floating_state_blocks_.count(state_ptr) == 1;
 }
 
 bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
 {
-    return isFactorRegisteredDerived(fac_ptr);
+    return factors_.count(fac_ptr) == 1 and isFactorRegisteredDerived(fac_ptr);
+}
+
+bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
+{
+    if (!isStateBlockRegistered(st))
+        return false;
+
+    if (isStateBlockFloating(st))
+        return st->isFixed();
+
+    return isStateBlockFixedDerived(st);
+}
+
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+{
+    if (!isStateBlockRegistered(st))
+        return false;
+
+    if (isStateBlockFloating(st))
+        return st->getLocalParametrization() == local_param;
+
+    return hasThisLocalParametrizationDerived(st, local_param);
+};
+
+bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
+{
+    if (!isStateBlockRegistered(st))
+        return false;
+
+    if (isStateBlockFloating(st))
+        return st->hasLocalParametrization();
+
+    return hasLocalParametrizationDerived(st);
+};
+
+int SolverManager::numFactors() const
+{
+    return factors_.size();
+}
+
+int SolverManager::numStateBlocks() const
+{
+    return state_blocks_.size() + floating_state_blocks_.size();
+}
+
+int SolverManager::numStateBlocksFloating() const
+{
+    return floating_state_blocks_.size();
 }
 
+double SolverManager::getPeriod() const
+{
+    return params_->period;
+}
 
 bool SolverManager::check(std::string prefix) const
 {
@@ -335,15 +526,32 @@ bool SolverManager::check(std::string prefix) const
             ok = false;
         }
 
-        // factor involving state block in factors_
-        for (auto fac : sb_fac_it->second)
+        // no factors involving state block
+        if (sb_fac_it->second.empty())
         {
-            if (factors_.count(fac) == 0)
+            WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix);
+            ok = false;
+        }
+        else
+        {
+            // factor involving state block in factors_
+            for (auto fac : sb_fac_it->second)
             {
-                WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
-                ok = false;
+                if (factors_.count(fac) == 0)
+                {
+                    WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+                    ok = false;
+                }
             }
         }
+
+        // can't be in both state_blocks_ and floating_state_blocks_
+        if (floating_state_blocks_.count(sb_fac_it->first) == 1)
+        {
+            WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix);
+            ok = false;
+        }
+
         sb_vec_it++;
         sb_fac_it++;
     }
@@ -362,10 +570,63 @@ bool SolverManager::check(std::string prefix) const
         }
     }
 
-    // checkDerived
+    // CHECK DERIVED ----------------------
     ok &= checkDerived(prefix);
 
+    // CHECK IF DERIVED IS UP TO DATE ----------------------
+    // state blocks registered in derived solver
+    for (auto sb : state_blocks_)
+        if (!isStateBlockRegisteredDerived(sb.first))
+        {
+            WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+            ok = false;
+        }
+
+    // factors registered in derived solver
+    for (auto fac : factors_)
+        if (!isFactorRegisteredDerived(fac))
+        {
+            WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+            ok = false;
+        }
+
+    // numbers
+    if (numStateBlocksDerived() != state_blocks_.size())
+    {
+        WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+        ok = false;
+    }
+    if (numFactorsDerived() != numFactors())
+    {
+        WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+        ok = false;
+    }
+
     return ok;
 }
 
+void SolverManager::printProfiling(std::ostream& _stream) const
+{
+    _stream <<"\nSolverManager:"
+            <<"\nTotal values:"
+            << "\n\tSolving state:          " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_
+            << "\n\t\tUpdate problem:   " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
+            << "\n\t\tSolver:           " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
+            << "\n\t\tUpdate state:     " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
+            << "\n\tSolving covariance:     " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_
+            <<"\nAverage:"
+            << "\n\tSolving state:          " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms"
+            << "\n\t\tUpdate problem:   " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms"
+            << "\n\t\tSolver:           " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
+            << "\n\t\tUpdate state:     " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms"
+            << "\n\tSolving covariance:     " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms"
+            <<"\nMax values:"
+            << "\n\tSolving state:          " << 1e-3*max_duration_total_.count() << " ms"
+            << "\n\t\tUpdate problem:   " << 1e-3*max_duration_update_.count() << " ms"
+            << "\n\t\tSolver:           " << 1e-3*max_duration_solver_.count() << " ms"
+            << "\n\t\tUpdate state:     " << 1e-3*max_duration_state_.count() << " ms"
+            << "\n\tSolving covariance:     " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
+
+}
+
 } // namespace wolf
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 908a2f4aabcddc90248ab99261c7d5824c54c826..f94353d93dff9a0425e984372dde89b0116c8b4d 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -1,4 +1,4 @@
-#include "core/ceres_wrapper/ceres_manager.h"
+#include "../../include/core/ceres_wrapper/solver_ceres.h"
 
 SolverManager::SolverManager()
 {
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 83cf44b26bdfcad7e415633ef6d8fe783564a75c..ce743db05620f2e5bea90fd6cb8512c9669d394f 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -4,7 +4,7 @@
 namespace wolf
 {
 
-StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
+StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
     state_block_map_.emplace(_sb_type, _sb);
@@ -54,4 +54,39 @@ void HasStateBlocks::perturb(double amplitude)
     }
 }
 
+void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+    if (_metric && _state_blocks)
+    {
+        for (const auto &key : getStructure())
+        {
+            auto sb = getStateBlock(key);
+            if (sb)
+                _stream << _tabs << "  " << key
+                        << "[" << (sb->isFixed() ? "Fix" : "Est")
+                        << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+                        << " @ " << sb << std::endl;
+        }
+    }
+    else if (_metric)
+    {
+        _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est")
+                << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )"
+                << std::endl;
+    }
+    else if (_state_blocks)
+    {
+        _stream << _tabs << "  " << "sb:";
+        for (const auto &key : getStructure())
+        {
+            const auto &sb = getStateBlock(key);
+            if (sb)
+                _stream << "    " << key
+                        << "[" << (sb->isFixed() ? "Fix" : "Est")
+                        << "] @ " << sb;
+        }
+        _stream << std::endl;
+    }
+}
+
 }
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index b2fa1c111e028445333236e01384144aeafc31a6..846b14a49934c70c0d3879efd03f454ffab74605 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -60,5 +60,9 @@ bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _
     return true;
 }
 
+bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, double tolerance)
+{
+    return _state.size() == global_size_;
+}
 } // namespace wolf
 
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 90b7f47c6b5ffa2a28374b5673827d325224430f..0f4efab6f1f093d8f1425d0855a453c6370f1c93 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -11,9 +11,8 @@ namespace wolf{
 VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
 {
     auto size_it = _sizes.begin();
-    for ( const auto& ckey : _structure)
+    for ( const auto& key : _structure)
     {
-        const auto& key     = string(1,ckey);
         const auto& size    = *size_it;
 
         this->emplace(key, VectorXd(size).setZero());
@@ -26,9 +25,8 @@ VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _stru
 {
     int index = 0;
     auto size_it = _sizes.begin();
-    for ( const auto& ckey : _structure)
+    for ( const auto& key : _structure)
     {
-        const auto& key     = string(1,ckey);
         const auto& size    = *size_it;
 
         (*this)[key]        = _v.segment(index,size);
@@ -40,9 +38,8 @@ VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _stru
 
 VectorComposite::VectorComposite (const StateStructure& _s)
 {
-    for (const auto& ckey : _s)
+    for (const auto& key : _s)
     {
-        const auto& key = string(1,ckey); // ckey is char
         this->emplace(key,VectorXd(0));
     }
 }
@@ -53,9 +50,8 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l
 
     auto vector_it = _vectors.begin();
 
-    for (const auto& ckey : _structure)
+    for (const auto& key : _structure)
     {
-        string key(1,ckey);
         this->emplace(key, *vector_it);
         vector_it++;
     }
@@ -67,9 +63,8 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
     // traverse once with unordered_map access
     std::vector<const VectorXd*> vp;
     unsigned int size = 0;
-    for (const auto& ckey : _structure)
+    for (const auto& key : _structure)
     {
-        std::string key(1,ckey); // ckey is char
         vp.push_back(&(this->at(key)));
         size +=  vp.back()->size();
     }
@@ -125,9 +120,8 @@ void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure,
 {
     int index = 0;
     auto size_it = _sizes.begin();
-    for ( const auto& ckey : _structure)
+    for ( const auto& key : _structure)
     {
-        const auto& key     = string(1,ckey); // ckey is char
         const auto& size    = *size_it;
 
         (*this)[key]        = _v.segment(index,size);
@@ -156,7 +150,7 @@ wolf::VectorComposite operator -(const wolf::VectorComposite &_x)
 
 ////// MATRIX COMPOSITE //////////
 
-bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk)
+bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk)
 {
     // check rows
     if (size_rows_.count(_row) == 0)
@@ -175,7 +169,7 @@ bool MatrixComposite::emplace(const std::string &_row, const std::string &_col,
     return true;
 }
 
-bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen::MatrixXd &_mat_blk) const
+bool MatrixComposite::at(const char &_row, const char &_col, Eigen::MatrixXd &_mat_blk) const
 {
     const auto &row_it = this->find(_row);
     if(row_it != this->end())
@@ -190,7 +184,7 @@ bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen
     return true;
 }
 
-Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col)
+Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col)
 {
     const auto &row_it = this->find(_row);
     assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
@@ -201,7 +195,7 @@ Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string
     return col_it->second;
 }
 
-const Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) const
+const Eigen::MatrixXd& MatrixComposite::at(const char &_row, const char &_col) const
 {
     const auto &row_it = this->find(_row);
     assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
@@ -287,7 +281,7 @@ MatrixComposite MatrixComposite::operator - (void) const
 
     for (const auto& pair_rkey_row : *this)
     {
-        m.unordered_map<string,unordered_map<string, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<string, MatrixXd>());
+        m.unordered_map<char,unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<char, MatrixXd>());
         for (const auto& pair_ckey_blk : pair_rkey_row.second)
         {
             m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second);
@@ -449,8 +443,8 @@ MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
 MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const
 {
 
-    std::unordered_map < string, unsigned int> indices_rows;
-    std::unordered_map < string, unsigned int> indices_cols;
+    std::unordered_map < char, unsigned int> indices_rows;
+    std::unordered_map < char, unsigned int> indices_cols;
     unsigned int rows, cols;
 
     sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
@@ -498,22 +492,20 @@ unsigned int MatrixComposite::cols() const
 
 void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
                                      const StateStructure &_struct_cols,
-                                     std::unordered_map<string, unsigned int> &_indices_rows,
-                                     std::unordered_map<string, unsigned int> &_indices_cols,
+                                     std::unordered_map<char, unsigned int> &_indices_rows,
+                                     std::unordered_map<char, unsigned int> &_indices_cols,
                                      unsigned int &_rows,
                                      unsigned int &_cols) const
 {
     _rows = 0;
     _cols = 0;
-    for (const auto& crow : _struct_rows)
+    for (const auto& row : _struct_rows)
     {
-        string row = string(1,crow); // crow is char
         _indices_rows[row]  = _rows;
         _rows              += size_rows_.at(row);
     }
-    for (const auto& ccol : _struct_cols)
+    for (const auto& col : _struct_cols)
     {
-        string col = string(1,ccol); // ccol is char
         _indices_cols[col]  = _cols;
         _cols              += size_cols_.at(col);
     }
@@ -521,16 +513,9 @@ void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
 
 MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure)
 {
-    for (const auto& rkey_char : _row_structure)
-    {
-        string rkey(1,rkey_char); // key was char
-        for (const auto& ckey_char : _col_structure)
-        {
-            string ckey(1,ckey_char); // key was char
-
+    for (const auto& rkey : _row_structure)
+        for (const auto& ckey : _col_structure)
             this->emplace(rkey, ckey, MatrixXd(0,0));
-        }
-    }
 }
 
 MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
@@ -542,15 +527,11 @@ MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
     assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
 
     auto rsize_it = _row_sizes.begin();
-    for (const auto& rkey_char : _row_structure)
+    for (const auto& rkey : _row_structure)
     {
-        string rkey(1,rkey_char); // key was char
-
         auto csize_it = _col_sizes.begin();
-        for (const auto& ckey_char : _col_structure)
+        for (const auto& ckey : _col_structure)
         {
-            string ckey(1,ckey_char); // key was char
-
             this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
 
             csize_it ++;
@@ -570,21 +551,17 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m,
 
     SizeEigen rindex = 0, cindex;
     auto rsize_it = _row_sizes.begin();
-    for (const auto& rkey_char : _row_structure)
+    for (const auto& rkey : _row_structure)
     {
         assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
 
-        string rkey(1,rkey_char); // key was char
-
         cindex = 0;
         auto csize_it = _col_sizes.begin();
 
-        for (const auto& ckey_char : _col_structure)
+        for (const auto& ckey : _col_structure)
         {
             assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns");
 
-            string ckey(1,ckey_char); // key was char
-
             this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
 
             cindex += *csize_it;
@@ -609,15 +586,11 @@ MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, con
     assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
 
     auto rsize_it = _row_sizes.begin();
-    for (const auto& rkey_char : _row_structure)
+    for (const auto& rkey : _row_structure)
     {
-        string rkey(1,rkey_char); // key was char
-
         auto csize_it = _col_sizes.begin();
-        for (const auto& ckey_char : _col_structure)
+        for (const auto& ckey : _col_structure)
         {
-            string ckey(1,ckey_char); // key was char
-
             m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
 
             csize_it ++;
@@ -639,7 +612,7 @@ MatrixComposite MatrixComposite::identity (const StateStructure& _structure, con
     while (rkey_it != _structure.end())
     {
 
-        const auto& rkey = string(1,*rkey_it);
+        const auto& rkey = *rkey_it;
         const auto rsize = *rsize_it;
 
         m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block
@@ -652,7 +625,7 @@ MatrixComposite MatrixComposite::identity (const StateStructure& _structure, con
 
         while (ckey_it != _structure.end())
         {
-            const auto& ckey = string(1,*ckey_it);
+            const auto& ckey  = *ckey_it;
             const auto& csize = *csize_it;
 
             m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
@@ -763,7 +736,7 @@ bool StateBlockComposite::isFixed() const
     return fixed;
 }
 
-unsigned int StateBlockComposite::remove(const std::string &_sb_type)
+unsigned int StateBlockComposite::remove(const char &_sb_type)
 {
     return state_block_map_.erase(_sb_type);
 }
@@ -778,7 +751,7 @@ bool StateBlockComposite::has(const StateBlockPtr &_sb) const
     return found;
 }
 
-StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
+StateBlockPtr StateBlockComposite::at(const char &_sb_type) const
 {
     auto it = state_block_map_.find(_sb_type);
     if (it != state_block_map_.end())
@@ -787,7 +760,7 @@ StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
         return nullptr;
 }
 
-void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &_sb)
+void StateBlockComposite::set(const char& _sb_type, const StateBlockPtr &_sb)
 {
     auto it = state_block_map_.find(_sb_type);
     assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
@@ -795,7 +768,7 @@ void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &
     it->second = _sb;
 }
 
-void StateBlockComposite::set(const std::string& _key, const VectorXd &_vec)
+void StateBlockComposite::set(const char& _key, const VectorXd &_vec)
 {
     auto it = state_block_map_.find(_key);
     assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
@@ -810,15 +783,14 @@ void StateBlockComposite::setVectors(const StateStructure& _structure, const std
     assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
 
     auto vec_it = _vectors.begin();
-    for (const auto ckey : _structure)
+    for (const auto key : _structure)
     {
-        string key(1,ckey);
         set (key, *vec_it);
         vec_it++;
     }
 }
 
-bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
+bool StateBlockComposite::key(const StateBlockPtr &_sb, char &_key) const
 {
     const auto& it = this->find(_sb);
 
@@ -831,12 +803,12 @@ bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
     }
     else
     {
-        _key = "";
+        _key ='0';
         return false;
     }
 }
 
-std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
+char StateBlockComposite::key(const StateBlockPtr& _sb) const
 {
     const auto& it = this->find(_sb);
 
@@ -845,7 +817,7 @@ std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
     if (found)
         return it->first;
     else
-        return "";
+        return '0';
 }
 
 
@@ -853,7 +825,7 @@ StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
 {
     const auto& it = std::find_if(state_block_map_.begin(),
                                   state_block_map_.end(),
-                                  [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+                                  [_sb](const std::pair<char, StateBlockPtr>& pair)
                                   {
                                       return pair.second == _sb;
                                   }
@@ -862,7 +834,7 @@ StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
     return it;
 }
 
-StateBlockPtr StateBlockComposite::add(const std::string &_sb_type, const StateBlockPtr &_sb)
+StateBlockPtr StateBlockComposite::add(const char &_sb_type, const StateBlockPtr &_sb)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
 
@@ -955,9 +927,8 @@ SizeEigen StateBlockComposite::stateSize() const
 SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
 {
     SizeEigen size = 0;
-    for (const auto& ckey : _structure)
+    for (const auto& key : _structure)
     {
-        string key(1,ckey);
         size += state_block_map_.at(key)->getSize();
     }
     return size;
@@ -968,9 +939,8 @@ VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) cons
     VectorXd x(stateSize(_structure));
     SizeEigen index = 0;
     SizeEigen size;
-    for (const auto& ckey : _structure)
+    for (const auto& key : _structure)
     {
-        string key(1,ckey);
         const auto& sb          = state_block_map_.at(key);
         size                    = sb->getSize();
         x.segment(index,size)   = sb->getState();
@@ -984,9 +954,8 @@ void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd
     _vector.resize(stateSize(_structure));
     SizeEigen index = 0;
     SizeEigen size;
-    for (const auto& ckey : _structure)
+    for (const auto& key : _structure)
     {
-        string key(1,ckey);
         const auto& sb              = state_block_map_.at(key);
         size                        = sb->getSize();
         _vector.segment(index,size) = sb->getState();
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index ad760c8c3ccf2942f734c256e7b04ed3d59aecf7..4a64613a3445bfead7565f38ae5527f01f2fde5c 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -4,11 +4,9 @@
 namespace wolf {
 
 TrajectoryBase::TrajectoryBase() :
-    NodeBase("TRAJECTORY", "TrajectoryBase"),
-    last_key_frame_ptr_(nullptr),
-    last_key_or_aux_frame_ptr_(nullptr)
+    NodeBase("TRAJECTORY", "TrajectoryBase")
 {
-//    WOLF_DEBUG("constructed T");
+    frame_map_ = FrameMap();
 }
 
 TrajectoryBase::~TrajectoryBase()
@@ -19,16 +17,9 @@ TrajectoryBase::~TrajectoryBase()
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.push_back(_frame_ptr);
+    assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
 
-    if (_frame_ptr->isKeyOrAux())
-    {
-        // sort
-        sortFrame(_frame_ptr);
-
-        // update last_estimated and last_key
-        updateLastFrames();
-    }
+    frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
 
     return _frame_ptr;
 }
@@ -36,116 +27,71 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.remove(_frame_ptr);
-
-    // update last_estimated and last_key
-    if (_frame_ptr->isKeyOrAux())
-        updateLastFrames();
+    // frame_map_.erase(_frame_ptr);
+    frame_map_.erase(_frame_ptr->getTimeStamp());
 }
 
 void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
 {
-	for(auto fr_ptr : getFrameList())
+	for(auto fr_ptr : *this)
 		fr_ptr->getFactorList(_fac_list);
 }
 
-void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
-{
-    moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
-}
-
-void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place)
-{
-    if (*_place != _frm_ptr)
-    {
-        frame_list_.remove(_frm_ptr);
-        frame_list_.insert(_place, _frm_ptr);
-        updateLastFrames();
-    }
-}
-
-FrameBaseConstIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
-{
-    for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
-        if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
-            return frm_rit.base();
-    return getFrameList().begin();
-}
 
-void TrajectoryBase::updateLastFrames()
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
 {
-    bool last_estimated_set = false;
-
-    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
-    for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
-        if ((*frm_rit)->isKeyOrAux())
-        {
-            if (!last_estimated_set)
-            {
-                last_key_or_aux_frame_ptr_ = (*frm_rit);
-                last_estimated_set = true;
-            }
-            if ((*frm_rit)->isKey())
-            {
-                last_key_frame_ptr_ = (*frm_rit);
-                break;
-            }
-        }
-}
-
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
-{
-    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
     FrameBasePtr closest_kf = nullptr;
-    double min_dt = 1e9;
-
-    for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
-        if ((*frm_rit)->isKey())
+    //If frame_map_ is empty then closestFrameToTimeStamp is meaningless
+    if(not frame_map_.empty())
+    {
+        //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
+        //In the std they fulfill the following property:
+        //        lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts.
+        // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1.
+
+        auto lower_bound = frame_map_.lower_bound(_ts);
+        if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second;
+        else
         {
-            double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
-            if (dt < min_dt)
-            {
-                min_dt = dt;
-                closest_kf = *frm_rit;
-            }
+            auto upper_bound = lower_bound;
+            //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the
+            //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases).
+            //Therefore, we need to decrease the lower_bound to the previous element
+            lower_bound = std::prev(lower_bound);
+
+            //If ub points to end() it means that the last frame is still less than _ts, therefore certainly
+            //it will be the closest one
+            if(upper_bound == this->end()) closest_kf = lower_bound->second;
             else
-                break;
-        }
-    return closest_kf;
-}
-
-FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
-{
-    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
-    FrameBasePtr closest_kf = nullptr;
-    double min_dt = 1e9;
-
-    for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
-        if ((*frm_rit)->isKeyOrAux())
-        {
-            double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
-            if (dt < min_dt)
             {
-                min_dt = dt;
-                closest_kf = *frm_rit;
+                //Easy stuff just calculate the distance return minimum
+                auto lb_diff = fabs(lower_bound->first - _ts);
+                auto ub_diff = fabs(upper_bound->first - _ts);
+                if(lb_diff < ub_diff)
+                {
+                    closest_kf = lower_bound->second;
+                }
+                else
+                {
+                    closest_kf = upper_bound->second;
+                }
             }
-            else
-                break;
         }
+    }
     return closest_kf;
 }
 
 void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
 {
-    _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameList().size()) + "F") : "")  << std::endl;
+    _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "")  << std::endl;
 }
 void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 1)
-        for (auto F : getFrameList())
-            F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
-
+        for (auto F : *this)
+            if (F)
+                F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
 CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
@@ -169,7 +115,7 @@ bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr,
     auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr);
     auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
     _log.compose(local_log);
-    for (auto F : getFrameList())
+    for (auto F : *this)
         F->check(_log, F, _verbose, _stream, _tabs + "  ");
     return _log.is_consistent_;
 }
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 5816567557bdd2ca5779615834a3f48a418bbabe..3c64b2ee87d2e26318788c4eddd547606edb6cb0 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -3,32 +3,22 @@
 namespace wolf
 {
 
-void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
+void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
 {
-    int n_kf(0);
-    FrameBasePtr first_KF(nullptr), second_KF(nullptr);
-    for (auto frm : getProblem()->getTrajectory()->getFrameList())
-    {
-        if (frm->isKey())
-        {
-            n_kf++;
-            if (first_KF == nullptr)
-                first_KF = frm;
-            else if (second_KF == nullptr)
-                second_KF = frm;
-        }
-    }
+    int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
 
-    // remove first KF if too many KF
-    if (n_kf > params_sw_->n_key_frames)
+    // remove first frame if too many frames
+    if (n_f > params_sw_->n_frames)
     {
-        WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
-        first_KF->remove(params_sw_->viral_remove_empty_parent);
-        if (params_sw_->fix_first_key_frame)
+        if (params_sw_->fix_first_frame)
         {
             WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
-            second_KF->fix();
+            auto second_frame = std::next(getProblem()->getTrajectory()->begin())->second;
+            if (second_frame)
+                second_frame->fix();
         }
+        WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
+        getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_empty_parent);
     }
 }
 
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2009cd89edf486af2efa892f58615a1e0fa05b0f
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -0,0 +1,86 @@
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/capture/capture_motion.h"
+#include "core/processor/processor_motion.h"
+
+namespace wolf
+{
+
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) :
+        TreeManagerSlidingWindow(_params),
+        params_swdr_(_params),
+        count_frames_(0)
+{
+    NodeBase::setType("TreeManagerSlidingWindowDualRate");
+}
+
+void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
+{
+    int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+
+    // recent segment not complete
+    if (n_f <= params_swdr_->n_frames_recent)
+        return;
+
+
+    // REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
+    if (count_frames_ != 0)
+    {
+        WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
+        FrameBasePtr remove_recent_frame    = std::next(getProblem()->getTrajectory()->rbegin(),
+                                                        params_swdr_->n_frames_recent)->second;
+        FrameBasePtr keep_recent_frame      = std::next(getProblem()->getTrajectory()->rbegin(),
+                                                        params_swdr_->n_frames_recent - 1)->second;
+
+        // compose motion captures for all processors motion
+        for (auto is_motion : getProblem()->getProcessorIsMotionList())
+        {
+            auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion);
+            if (proc_motion == nullptr)
+                continue;
+
+            auto cap_prev = std::static_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
+            auto cap_next = std::static_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));
+
+            // merge captures (if exist)
+            if (cap_prev and cap_next)
+            {
+                WOLF_DEBUG("TreeManagerSlidingWindow merging capture ", cap_prev->id(), " with ", cap_next->id());
+                assert(cap_next->getOriginCapture() == cap_prev);
+                proc_motion->mergeCaptures(cap_prev, cap_next);
+            }
+
+        }
+
+        // remove frame
+        remove_recent_frame->remove(params_swdr_->viral_remove_empty_parent);
+    }
+    // REMOVE OLDEST FRAME: when first recent frame is kept, remove oldest frame (if max frames reached)
+    else if (n_f > params_swdr_->n_frames)
+    {
+        if (params_swdr_->fix_first_frame)
+        {
+            WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
+            auto second_frame = *std::next(getProblem()->getTrajectory()->begin());
+            if (second_frame)
+                second_frame->fix();
+        }
+        WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
+        getProblem()->getTrajectory()->getFirstFrame()->remove(params_swdr_->viral_remove_empty_parent);
+    }
+
+    // iterate counter
+    count_frames_++;
+    if (count_frames_ == params_swdr_->rate_old_frames)
+        count_frames_ = 0;
+}
+
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowDualRate);
+} // namespace wolf
+
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index 82057b000ffaf1eb34b674dbe5ac9a224b5efb7a..f07cc4d559c911e9cfc9e4ecb2797417c9b21201 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -345,6 +345,12 @@ void ParserYaml::updateActiveName(std::string _tag)
 {
     active_name_ = _tag;
 }
+/*
+** @brief This function is responsible for parsing the first level of the YAML file.
+** The first level here can be thought as the entry point of the YAML file. Since we impose a certain structure
+** this function is responsible for enforcing said structure.
+**
+ */
 void ParserYaml::parseFirstLevel(YAML::Node _n, std::string _file)
 {
 
@@ -482,13 +488,13 @@ void ParserYaml::parse()
     {
         std::vector<std::string> tags = std::vector<std::string>();
         tags.push_back("ROS subscriber");
-        walkTreeR(it.n_, tags, "ROS subscriber/" + it.topic_);
+        walkTreeR(it.n_, tags, "ROS subscriber/" + it.type_ + " - " + it.topic_);
     }
     for (auto it : publisher_managers_)
     {
         std::vector<std::string> tags = std::vector<std::string>();
         tags.push_back("ROS publisher");
-        walkTreeR(it.n_, tags, "ROS publisher/" + it.topic_);
+        walkTreeR(it.n_, tags, "ROS publisher/" + it.type_ + " - " + it.topic_);
     }
     std::list<std::string> plugins;
     std::list<std::string> packages_subscriber, packages_publisher;
@@ -550,6 +556,9 @@ void ParserYaml::parse()
     }
     parsing_file_.pop();
 }
+/*
+** @brief This function gives the ability to run the parser without enforcing the wolf YAML structure.
+ */
 void ParserYaml::parse_freely()
 {
     parsing_file_.push(generatePath(file_));
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index 9c21e3842304e475146194bc04da7be9f73de396..bd168ee20b162426a85d7b1b78e508f2c00a8fe1 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -34,7 +34,6 @@ static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _f
         YAML::Node keyframe_vote = config["keyframe_vote"];
 
         params->voting_active       = keyframe_vote["voting_active"]    .as<bool>();
-        params->voting_aux_active   = keyframe_vote["voting_aux_active"].as<bool>();
         params->max_time_span       = keyframe_vote["max_time_span"]    .as<double>();
         params->max_buff_length     = keyframe_vote["max_buff_length"]  .as<SizeEigen>();
         params->dist_traveled       = keyframe_vote["dist_traveled"]    .as<double>();
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 05717bad197c33e241146a8cf4d08fc337d99ea2..4363e38a4d4df67a73ce92086b7a3ae071c4a561 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,7 +28,7 @@ include_directories(${GTEST_INCLUDE_DIRS})
 #                                                         #
 # Create a specific test executable for gtest_example     #
 wolf_add_gtest(gtest_example gtest_example.cpp)           #
-target_link_libraries(gtest_example ${PROJECT_NAME})      #
+target_link_libraries(gtest_example ${PLUGIN_NAME})      #
 #                                                         #
 ###########################################################
 set(SRC_DUMMY
@@ -36,7 +36,7 @@ set(SRC_DUMMY
   dummy/processor_tracker_landmark_dummy.cpp
   )
 add_library(dummy SHARED ${SRC_DUMMY})
-target_link_libraries(dummy ${PROJECT_NAME})
+target_link_libraries(dummy ${PLUGIN_NAME})
 ################# ADD YOUR TESTS BELOW ####################
 #                                                         #
 #           ==== IN ALPHABETICAL ORDER! ====              #
@@ -46,221 +46,231 @@ target_link_libraries(dummy ${PROJECT_NAME})
 
 # CaptureBase class test
 wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PROJECT_NAME})
+target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
 
 # Converter from String to various types used by the parameters server
 wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PROJECT_NAME})
+target_link_libraries(gtest_converter ${PLUGIN_NAME})
 
 # FactorBase class test
 wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
-target_link_libraries(gtest_factor_base ${PROJECT_NAME})
+target_link_libraries(gtest_factor_base ${PLUGIN_NAME})
 
 # FactorAutodiff class test
 wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
+target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
 
 # FactoryStateBlock factory test
 wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
-target_link_libraries(gtest_factory_state_block ${PROJECT_NAME})
+target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
 
 # FeatureBase classes test
 wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
+target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
 
 # Node Emplace test
 wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
 
 # FeatureBase classes test
 wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-target_link_libraries(gtest_feature_base ${PROJECT_NAME})
+target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
 
 # FrameBase classes test
 wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PROJECT_NAME})
+target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
 
 # HasStateBlocks classes test
 wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
-target_link_libraries(gtest_has_state_blocks ${PROJECT_NAME})
+target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
 
 # LocalParametrizationXxx classes test
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PROJECT_NAME})
+target_link_libraries(gtest_local_param ${PLUGIN_NAME})
 
 # Logging test
 wolf_add_gtest(gtest_logging gtest_logging.cpp)
-target_link_libraries(gtest_logging ${PROJECT_NAME})
+target_link_libraries(gtest_logging ${PLUGIN_NAME})
 
 # MotionBuffer class test
 wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
+target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
 
 # PackKFBuffer
 wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
-target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_pack_KF_buffer ${PLUGIN_NAME} dummy)
 
 # Parameters server
 wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
-target_link_libraries(gtest_param_server ${PROJECT_NAME})
+target_link_libraries(gtest_param_server ${PLUGIN_NAME})
 
 # YAML parser
 wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-target_link_libraries(gtest_parser_yaml ${PROJECT_NAME})
+target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME})
 
 # Problem class test
 wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
 
 # ProcessorBase class test
 wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
 
 # ProcessorMotion class test
 wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
+target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
   
 # Rotation test
 wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
 
 # SE3 test
 wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
-target_link_libraries(gtest_SE3 ${PROJECT_NAME})
+target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
 
 # SensorBase test
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
+target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})
 
 # shared_from_this test
 wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+target_link_libraries(gtest_shared_from_this ${PLUGIN_NAME})
 
 # SolverManager test
 wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
+target_link_libraries(gtest_solver_manager ${PLUGIN_NAME})
+
+# SolverManagerMultithread test
+wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp)
+target_link_libraries(gtest_solver_manager_multithread ${PLUGIN_NAME})
 
 # StateBlock class test
 wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
-target_link_libraries(gtest_state_block ${PROJECT_NAME})
+target_link_libraries(gtest_state_block ${PLUGIN_NAME})
 
 # StateBlock class test
 wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
-target_link_libraries(gtest_state_composite ${PROJECT_NAME})
+target_link_libraries(gtest_state_composite ${PLUGIN_NAME})
 
 # TimeStamp class test
 wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
-target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
+target_link_libraries(gtest_time_stamp ${PLUGIN_NAME})
 
 # TrackMatrix class test
 wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
+target_link_libraries(gtest_track_matrix ${PLUGIN_NAME})
 
 # TrajectoryBase class test
 wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-target_link_libraries(gtest_trajectory ${PROJECT_NAME})
+target_link_libraries(gtest_trajectory ${PLUGIN_NAME})
 
 # TreeManager class test
 wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
-target_link_libraries(gtest_tree_manager ${PROJECT_NAME})
+target_link_libraries(gtest_tree_manager ${PLUGIN_NAME})
 
 # ------- Now Derived classes ----------
 
-IF (Ceres_FOUND)
-# CeresManager test
-wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp)
-target_link_libraries(gtest_ceres_manager ${PROJECT_NAME})
-ENDIF(Ceres_FOUND)
-
 # FactorAbs(P/O/V) classes test
 wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
+target_link_libraries(gtest_factor_absolute ${PLUGIN_NAME})
 
 # FactorAutodiffDistance3d test
 wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
-target_link_libraries(gtest_factor_autodiff_distance_3d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME})
 
 # FactorBlockDifference class test
 wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
-target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
+target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
 
 # FactorOdom3d class test
 wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
-target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME})
+target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
 
 # FactorOdom2d class test
 wolf_add_gtest(gtest_factor_odom_2d gtest_factor_odom_2d.cpp)
-target_link_libraries(gtest_factor_odom_2d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_odom_2d ${PLUGIN_NAME})
 
 # FactorOdom3d class test
 wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
-target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_odom_3d ${PLUGIN_NAME})
 
 # FactorPose2d class test
 wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
-target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
 
 # FactorPose3d class test
 wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
-target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
 
 # FactorRelativePose2dWithExtrinsics class test
 wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PROJECT_NAME})
+target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME})
 
 # FactorRelative2dAnalytic class test
 wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp)
-target_link_libraries(gtest_factor_relative_2d_analytic ${PROJECT_NAME})
+target_link_libraries(gtest_factor_relative_2d_analytic ${PLUGIN_NAME})
 
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
+target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
 
 # Map yaml save/load test
 wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
-target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
+target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
 
 # Parameter prior test
 wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PROJECT_NAME})
+target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
 
 # ProcessorDiffDriveSelfcalib class test
 wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
-target_link_libraries(gtest_processor_diff_drive ${PROJECT_NAME})
+target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
 
 # ProcessorLoopClosureBase class test
 wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
-target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME})
+target_link_libraries(gtest_processor_loopclosure ${PLUGIN_NAME})
 
 # ProcessorFrameNearestNeighborFilter class test
 # wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
-# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PROJECT_NAME})
+# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME})
 
 # ProcessorMotion in 2d
 wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-target_link_libraries(gtest_odom_2d ${PROJECT_NAME})
+target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
 
 # ProcessorOdom3d class test
 wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-target_link_libraries(gtest_processor_odom_3d ${PROJECT_NAME})
+target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
 
 # ProcessorTrackerFeatureDummy class test
 wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
-target_link_libraries(gtest_processor_tracker_feature_dummy ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy)
 
 # ProcessorTrackerLandmarkDummy class test
 wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
-target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dummy)
 
 # SensorDiffDriveSelfcalib class test
 wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
-target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME})
+target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
+
+IF (Ceres_FOUND)
+	# SolverCeres test
+	wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
+	target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME})
+	
+	# SolverCeresMultithread test
+	wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
+	target_link_libraries(gtest_solver_ceres_multithread ${PLUGIN_NAME})
+ENDIF(Ceres_FOUND)
 
 # TreeManagerSlidingWindow class test
 wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
-target_link_libraries(gtest_tree_manager_sliding_window ${PROJECT_NAME})
+target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME})
+
+# TreeManagerSlidingWindowDualRate class test
+wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME})
 
 # yaml conversions
 wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
-target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
-
-
+target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME})
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 15db720a3c42f8151dd8745dc35d0232fc0fe885..aab56cd1ab532a6a7d3e4b876f2de6dea0ea1b7b 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy);
 class SolverManagerDummy : public SolverManager
 {
     public:
-        std::list<FactorBasePtr> factors_;
+        std::set<FactorBasePtr> factors_derived_;
         std::map<StateBlockPtr,bool> state_block_fixed_;
         std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
 
@@ -26,35 +26,34 @@ class SolverManagerDummy : public SolverManager
         {
         };
 
-        bool isStateBlockRegistered(const StateBlockPtr& st) override
+        bool isStateBlockFixedDerived(const StateBlockPtr& st) override
         {
-            return state_blocks_.find(st)!=state_blocks_.end();
+            return state_block_fixed_.at(st);
         };
 
-        bool isStateBlockFixed(const StateBlockPtr& st) const
+        bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
         {
-            return state_block_fixed_.at(st);
+            return state_block_local_param_.at(st) == local_param;
         };
 
-        bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
+        bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
         {
-            return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+            return state_block_local_param_.at(st) != nullptr;
         };
 
-        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+        virtual int numStateBlocksDerived() const override
         {
-            return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
-        };
+            return state_block_fixed_.size();
+        }
 
-        bool hasLocalParametrization(const StateBlockPtr& st) const
+        virtual int numFactorsDerived() const override
         {
-            return state_block_local_param_.find(st) != state_block_local_param_.end();
+            return factors_derived_.size();
         };
 
-        void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
-        void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {};
-        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;};
-        bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;};
+        bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;};
+        bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;};
+
 
         // The following are dummy implementations
         bool    hasConverged() override  { return true;      }
@@ -62,19 +61,17 @@ class SolverManagerDummy : public SolverManager
         double  initialCost() override   { return double(1); }
         double  finalCost() override     { return double(0); }
 
-
-
     protected:
 
         bool checkDerived(std::string prefix="") const override {return true;}
         std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
         void addFactorDerived(const FactorBasePtr& fac_ptr) override
         {
-            factors_.push_back(fac_ptr);
+            factors_derived_.insert(fac_ptr);
         };
         void removeFactorDerived(const FactorBasePtr& fac_ptr) override
         {
-            factors_.remove(fac_ptr);
+            factors_derived_.erase(fac_ptr);
         };
         void addStateBlockDerived(const StateBlockPtr& state_ptr) override
         {
@@ -92,10 +89,16 @@ class SolverManagerDummy : public SolverManager
         };
         void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
         {
-            if (state_ptr->getLocalParametrization() == nullptr)
-                state_block_local_param_.erase(state_ptr);
-            else
-                state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+            state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+        };
+        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
+        {
+            return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
+        };
+
+        bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
+        {
+            return factors_derived_.count(fac_ptr) == 1;
         };
 };
 
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index 662c7d15677173fc302c02b10b1f660297753daf..1e7e70f2ee476404cb96b070e72f280c7a570a6e 100644
--- a/test/dummy/tree_manager_dummy.h
+++ b/test/dummy/tree_manager_dummy.h
@@ -20,7 +20,7 @@ struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
 
     bool toy_param;
 
-    std::string print() const
+    std::string print() const override
     {
         return ParamsTreeManagerBase::print() + "\n"
                + "toy_param: " + std::to_string(toy_param) + "\n";
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index 27acfea3069b92076bd30e4abd4ac28d787147bf..a99f7a19551f3e450e6b4958ded294db8980c62c 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -146,15 +146,15 @@ TEST(SE3, composeVectorComposite)
 
     VectorComposite x1, x2, xc("PO", {3,4});
 
-    x1.emplace("P", p1);
-    x1.emplace("O", q1.coeffs());
-    x2.emplace("P", p2);
-    x2.emplace("O", q2.coeffs());
+    x1.emplace('P', p1);
+    x1.emplace('O', q1.coeffs());
+    x2.emplace('P', p2);
+    x2.emplace('O', q2.coeffs());
 
     compose(x1, x2, xc);
 
-    ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
-    ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+    ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+    ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
 }
 
 TEST(SE3, composeVectorComposite_Jacobians)
@@ -174,26 +174,26 @@ TEST(SE3, composeVectorComposite_Jacobians)
     VectorComposite x1, x2, xc("PO", {3,4});
     MatrixComposite J_xc_x1, J_xc_x2;
 
-    x1.emplace("P", p1);
-    x1.emplace("O", q1.coeffs());
-    x2.emplace("P", p2);
-    x2.emplace("O", q2.coeffs());
+    x1.emplace('P', p1);
+    x1.emplace('O', q1.coeffs());
+    x2.emplace('P', p2);
+    x2.emplace('O', q2.coeffs());
 
     // we'll do xc = x1 * x2 and obtain Jacobians
     compose(x1, x2, xc, J_xc_x1, J_xc_x2);
 
-    ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
-    ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+    ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+    ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
 
-    ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "P"), Matrix3d::Identity()   , 1e-8);
-    ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "O"), - R1 * skew(p2)        , 1e-8);
-    ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "P"), Matrix3d::Zero()       , 1e-8);
-    ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "O"), R2.transpose()         , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity()   , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2)        , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero()       , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose()         , 1e-8);
 
-    ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "P"), R1                     , 1e-8);
-    ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "O"), Matrix3d::Zero()       , 1e-8);
-    ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "P"), Matrix3d::Zero()       , 1e-8);
-    ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "O"), Matrix3d::Identity()   , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1                     , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero()       , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero()       , 1e-8);
+    ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity()   , 1e-8);
 }
 
 TEST(SE3, exp_0_Composite)
@@ -205,13 +205,13 @@ TEST(SE3, exp_0_Composite)
 
     VectorComposite tau;
 
-    tau.emplace("P", p);
-    tau.emplace("O", theta);
+    tau.emplace('P', p);
+    tau.emplace('O', theta);
 
     VectorComposite x = SE3::exp(tau);
 
-    ASSERT_MATRIX_APPROX(x.at("P"), Vector3d::Zero(), 1e-8);
-    ASSERT_MATRIX_APPROX(x.at("O"), Quaterniond::Identity().coeffs(), 1e-8);
+    ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
+    ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
 
 }
 
@@ -223,20 +223,20 @@ TEST(SE3, plus_0_Composite)
     Vector4d q; q.setRandom().normalized();
 
     VectorComposite x;
-    x.emplace("P", p);
-    x.emplace("O", q);
+    x.emplace('P', p);
+    x.emplace('O', q);
 
     Vector3d dp; dp.setZero();
     Vector3d dtheta; dtheta.setZero();
 
     VectorComposite tau;
-    tau.emplace("P", dp);
-    tau.emplace("O", dtheta);
+    tau.emplace('P', dp);
+    tau.emplace('O', dtheta);
 
     VectorComposite res = plus(x, tau);
 
-    ASSERT_MATRIX_APPROX(res.at("P"), p, 1e-8);
-    ASSERT_MATRIX_APPROX(res.at("O"), q, 1e-8);
+    ASSERT_MATRIX_APPROX(res.at('P'), p, 1e-8);
+    ASSERT_MATRIX_APPROX(res.at('O'), q, 1e-8);
 }
 
 TEST(SE3, interpolate_I_xyz)
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 4c004cd72e14e9e69bb343e288ab86e3a5b9354e..3a0cbc4555c88bd40f0a656e2a73ff2a619654b6 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -100,6 +100,93 @@ TEST(CaptureBase, process)
     ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
 }
 
+TEST(CaptureBase, move_from_F_to_KF)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+    auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+    auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+    auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
+
+    ASSERT_EQ(KF->getCaptureList().size(), 1);
+    ASSERT_EQ(F->getCaptureList().size(), 1);
+
+    C->move(KF);
+
+    ASSERT_EQ(KF->getCaptureList().size(), 2);
+    ASSERT_EQ(F->getCaptureList().size(), 0);
+    ASSERT_TRUE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_F_to_null)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    FrameBasePtr F0 = nullptr;
+
+    auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+    auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
+
+    ASSERT_EQ(F->getCaptureList().size(), 1);
+
+    C->move(F0);
+
+    ASSERT_EQ(F->getCaptureList().size(), 0);
+    ASSERT_FALSE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_null_to_KF)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+    auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+    auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
+
+    ASSERT_EQ(KF->getCaptureList().size(), 1);
+
+    C->move(KF);
+
+    ASSERT_EQ(KF->getCaptureList().size(), 2);
+    ASSERT_TRUE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_null_to_F)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+    auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
+
+    C->move(F);
+
+    ASSERT_EQ(F->getCaptureList().size(), 1);
+
+    ASSERT_FALSE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_KF_to_F)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+    auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+    auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+    ASSERT_DEATH(C->move(F), "");
+}
+
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
deleted file mode 100644
index 5586332e6eb0470e8ffef1c9b0eb2d9d2fd1b313..0000000000000000000000000000000000000000
--- a/test/gtest_ceres_manager.cpp
+++ /dev/null
@@ -1,634 +0,0 @@
-/*
- * gtest_ceres_manager.cpp
- *
- *  Created on: Jun, 2018
- *      Author: jvallve
- */
-
-#include "core/utils/utils_gtest.h"
-
-
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/capture/capture_void.h"
-#include "core/factor/factor_pose_2d.h"
-#include "core/factor/factor_quaternion_absolute.h"
-#include "core/solver/solver_manager.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/state_block/local_parametrization_angle.h"
-#include "core/state_block/local_parametrization_quaternion.h"
-
-#include "ceres/ceres.h"
-
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-WOLF_PTR_TYPEDEFS(CeresManagerWrapper);
-class CeresManagerWrapper : public CeresManager
-{
-    public:
-        CeresManagerWrapper(const ProblemPtr& wolf_problem) :
-            CeresManager(wolf_problem)
-        {
-        };
-
-        bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st)
-        {
-            return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
-        };
-
-        bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
-        {
-            return state_blocks_.find(st)!=state_blocks_.end();
-        };
-
-        bool isStateBlockFixed(const StateBlockPtr& st)
-        {
-            return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
-        };
-
-        int numStateBlocks()
-        {
-            return ceres_problem_->NumParameterBlocks();
-        };
-
-        int numFactors()
-        {
-            return ceres_problem_->NumResidualBlocks();
-        };
-
-        bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
-        {
-            return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
-        };
-
-        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
-        {
-            return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
-                   state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
-                   ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
-        };
-
-        bool hasLocalParametrization(const StateBlockPtr& st) const
-        {
-            return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
-        };
-
-};
-
-TEST(CeresManager, Create)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // check double ointers to branches
-    ASSERT_EQ(P, ceres_manager_ptr->getProblem());
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, DoubleAddStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // add stateblock again
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, UpdateStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock unfixed
-    ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
-
-    // Fix frame
-    sb_ptr->fix();
-
-    // update solver manager
-    ceres_manager_ptr->update();
-
-    // check stateblock fixed
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddUpdateStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // Fix state block
-    sb_ptr->fix();
-
-    // update solver manager
-    ceres_manager_ptr->update();
-
-    // check stateblock fixed
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, RemoveStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
-
-    // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
-    ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddRemoveStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check no stateblocks
-    ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
-    ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, RemoveUpdateStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, DoubleRemoveStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
-
-    // update solver manager
-    ceres_manager_ptr->update();
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddFactor)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY,  0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check factor
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, DoubleAddFactor)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
-
-    // add factor again
-    P->notifyFactor(c,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check factor
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, RemoveFactor)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // remove factor
-    P->notifyFactor(c,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check factor
-    ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddRemoveFactor)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
-
-    // remove factor
-    P->notifyFactor(c,REMOVE);
-
-    ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check factor
-    ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, DoubleRemoveFactor)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // remove factor
-    P->notifyFactor(c,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // remove factor
-    P->notifyFactor(c,REMOVE);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check factor
-    ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddStateBlockLocalParam)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector1d state; state << 1;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // Local param
-    LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrization(local_param_ptr);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, RemoveLocalParam)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector1d state; state << 1;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // Local param
-    LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrization(local_param_ptr);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // Remove local param
-    sb_ptr->removeLocalParametrization();
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, AddLocalParam)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create State block
-    Vector1d state; state << 1;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add stateblock
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
-
-    // Local param
-    LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrization(local_param_ptr);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check stateblock
-    ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
-    ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, FactorsRemoveLocalParam)
-{
-    ProblemPtr P = Problem::create("PO", 3);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) 2 factors quaternion
-    FrameBasePtr                    F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
-    FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check local param
-    ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
-    ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
-
-    // check factor
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
-    // remove local param
-    F->getO()->removeLocalParametrization();
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check local param
-    ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
-
-    // check factor
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-TEST(CeresManager, FactorsUpdateLocalParam)
-{
-    ProblemPtr P = Problem::create("PO", 3);
-    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
-
-    // Create (and add) 2 factors quaternion
-    FrameBasePtr                    F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
-    FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check local param
-    ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
-    ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
-
-    // check factor
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
-    // remove local param
-    LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
-    F->getO()->setLocalParametrization(local_param_ptr);
-
-    // update solver
-    ceres_manager_ptr->update();
-
-    // check local param
-    ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
-    ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr));
-
-    // check factor
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
-    ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
-    ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
-
-    // run ceres manager check
-    ASSERT_TRUE(ceres_manager_ptr->check());
-}
-
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 2c28c2bd442bc8675bf5e71fb82d97141cac8390..21ed3167006049866bcbc481ec5bfa81c760a23c 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -42,8 +42,8 @@ TEST(Emplace, Frame)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+    FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 }
 
 TEST(Emplace, Processor)
@@ -67,12 +67,12 @@ TEST(Emplace, Capture)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
     ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
 }
 
@@ -81,17 +81,17 @@ TEST(Emplace, Feature)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
     ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
     auto cov = Eigen::MatrixXd::Identity(2,2);
     FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXd(2), cov);
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
     ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
 }
 TEST(Emplace, Factor)
@@ -99,17 +99,17 @@ TEST(Emplace, Factor)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
     ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
     auto cov = Eigen::MatrixXd::Identity(2,2);
     auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
     ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
     auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
     ASSERT_NE(nullptr, ftr->getFactorList().front().get());
@@ -119,7 +119,7 @@ TEST(Emplace, EmplaceDerived)
 {
     ProblemPtr P = Problem::create("POV", 3);
 
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
     auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
     auto cov = Eigen::MatrixXd::Identity(2,2);
     auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -130,7 +130,7 @@ TEST(Emplace, EmplaceDerived)
 
     auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
     ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
-    ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+    ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
 }
 TEST(Emplace, Nullpointer)
 {
@@ -141,7 +141,7 @@ TEST(Emplace, ReturnDerived)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
     auto cov = Eigen::MatrixXd::Identity(2,2);
     auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 82b56da523c70dff134908ba3e63adf97493d914..dc3551d2c79d47973e4dca81d4046fa1f669661d 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -5,12 +5,12 @@
  *      \author: datchuth
  */
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/utils/utils_gtest.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
 #include "core/capture/capture_motion.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -32,10 +32,10 @@ Vector10d x0 = pose9toPose10(pose + Vector9d::Random()*0.25);
 
 // Problem and solver
 ProblemPtr problem_ptr = Problem::create("POV", 3);
-CeresManager ceres_mgr(problem_ptr);
+SolverCeres solver(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() );
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, problem_ptr->stateZero() );
 
 // Capture
 // auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
@@ -59,7 +59,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
 
     //only orientation is constrained
     ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
@@ -75,7 +75,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
 
     //only orientation is constrained
     ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
@@ -93,7 +93,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
 
     //only velocity is constrained
     ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
@@ -111,7 +111,7 @@ TEST(FactorQuatAbs, fac_block_abs_o)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
 
     //only velocity is constrained
     ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 0acd77891bf1bae60fe41575edec629e9864999b..f4e3108d6bdf2e129358b851e82772dc5cffae3a 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -350,8 +350,11 @@ TEST(FactorAutodiff, AutodiffDummy12)
 
 TEST(FactorAutodiff, EmplaceOdom2d)
 {
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1));
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -377,12 +380,14 @@ TEST(FactorAutodiff, EmplaceOdom2d)
 
 TEST(FactorAutodiff, ResidualOdom2d)
 {
+    ProblemPtr problem = Problem::create("PO", 2);
+
     Eigen::Vector3d f1_pose, f2_pose;
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -421,12 +426,14 @@ TEST(FactorAutodiff, ResidualOdom2d)
 
 TEST(FactorAutodiff, JacobianOdom2d)
 {
+    ProblemPtr problem = Problem::create("PO", 2);
+
     Eigen::Vector3d f1_pose, f2_pose;
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -499,12 +506,14 @@ TEST(FactorAutodiff, JacobianOdom2d)
 
 TEST(FactorAutodiff, AutodiffVsAnalytic)
 {
+    ProblemPtr problem = Problem::create("PO", 2);
+
     Eigen::Vector3d f1_pose, f2_pose;
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 1a3f492384290e2bb14e8f01af153aa61abc1193..fa3d365424b3d62aa56eb6e6d186163251a594ad 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -5,10 +5,10 @@
  *      \author: jsola
  */
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/factor/factor_autodiff_distance_3d.h"
 #include "core/problem/problem.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 #include "core/math/rotations.h"
 
 #include "core/utils/utils_gtest.h"
@@ -34,7 +34,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test
         Matrix1d dist_cov;
 
         ProblemPtr problem;
-        CeresManagerPtr ceres_manager;
+        SolverCeresPtr solver;
 
         void SetUp() override
         {
@@ -53,11 +53,11 @@ class FactorAutodiffDistance3d_Test : public testing::Test
             dist_cov = Matrix1d(0.01);
 
             problem = Problem::create("PO", 3);
-            ceres_manager = std::make_shared<CeresManager>(problem);
+            solver = std::make_shared<SolverCeres>(problem);
 
-            F1 = problem->emplaceFrame        (KEY, 1.0, pose1);
+            F1 = problem->emplaceFrame        (1.0, pose1);
 
-            F2 = problem->emplaceFrame        (KEY, 2.0, pose2);
+            F2 = problem->emplaceFrame        (2.0, pose2);
             C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
         }
 
@@ -96,7 +96,7 @@ TEST_F(FactorAutodiffDistance3d_Test, solve)
     f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
     c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
 
-    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
+    std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET);
 
     // Check distance between F1 and F2 positions -- must match the measurement
     ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index e1e035bf0ae084d309b51db84b4c49ac62440924..79663676a51ea74c3ea2fc23f5c32c5e18e1cf30 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -24,8 +24,8 @@ class FactorBaseTest : public testing::Test
 
         void SetUp() override
         {
-            F0 = std::make_shared<FrameBase>(KEY, 0.0, nullptr);
-            F1 = std::make_shared<FrameBase>(KEY, 1.0, nullptr);
+            F0 = std::make_shared<FrameBase>(0.0, nullptr);
+            F1 = std::make_shared<FrameBase>(1.0, nullptr);
             C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
             C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
             f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 9c739965701b6236dcff93fbafba1bd12f90eddb..1dbf21b97b609ae212b1e88cf16ef993c2e742dc 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -8,9 +8,9 @@
 #include "core/utils/utils_gtest.h"
 
 #include <Eigen/Dense>
-#include "core/problem/problem.h"
-#include <core/ceres_wrapper/ceres_manager.h>
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/problem/problem.h"
 #include "core/frame/frame_base.h"
 #include "core/capture/capture_base.h"
 #include "core/feature/feature_base.h"
@@ -30,7 +30,7 @@ class FixtureFactorBlockDifference : public testing::Test
 {
     public:
         ProblemPtr problem_;
-        CeresManagerPtr ceres_manager_;
+        SolverCeresPtr solver_;
         FrameBasePtr KF0_;
         FrameBasePtr KF1_;
         CaptureBasePtr Cap_;
@@ -43,8 +43,7 @@ class FixtureFactorBlockDifference : public testing::Test
         {
             // Problem and solver
             problem_ = Problem::create("POV", 3);
-            ceres::Solver::Options ceres_options;
-            ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
+            solver_ = std::make_shared<SolverCeres>(problem_);
 
             TimeStamp t0(0);
             TimeStamp t1(1);
@@ -59,7 +58,7 @@ class FixtureFactorBlockDifference : public testing::Test
             //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
             //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
 
-            KF1_ = problem_->emplaceFrame(KEY, t1, problem_->stateZero());
+            KF1_ = problem_->emplaceFrame(t1, problem_->stateZero());
 
             Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
         }
@@ -84,7 +83,7 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
     KF0_->getP()->setState(Vector3d::Random());
     KF1_->getP()->setState(Vector3d::Random());
     
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
 }
@@ -98,7 +97,7 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
     KF0_->getV()->setState(Vector3d::Random());
     KF1_->getV()->setState(Vector3d::Random());
 
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
 }
@@ -114,7 +113,7 @@ TEST_F(FixtureFactorBlockDifference, DiffP)
     KF0_->getP()->setState(Vector3d::Random());
     KF1_->getP()->setState(Vector3d::Random());
 
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8);
 }
@@ -130,7 +129,7 @@ TEST_F(FixtureFactorBlockDifference, DiffV)
     KF0_->getV()->setState(Vector3d::Random());
     KF1_->getV()->setState(Vector3d::Random());
 
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diff, 1e-8);
 }
@@ -151,7 +150,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
     KF0_->getP()->setState(Vector3d::Random());
     KF1_->getP()->setState(Vector3d::Random());
 
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8);
 }
@@ -171,7 +170,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
     KF0_->getP()->setState(Vector3d::Random());
     KF1_->getP()->setState(Vector3d::Random());
 
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<2>() - KF0_->getP()->getState().head<2>(), diff, 1e-8);
 }
@@ -192,7 +191,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
     KF1_->getP()->setState(Vector3d::Random());
 
     problem_->print(4,true,true,true);
-    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
     std::cout << report << std::endl;
     problem_->print(4,true,true,true);
 
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index f2e8e5f6014811ea3b99c2843fa4b0e8e94c5b72..4f3ece70d92af1deeb923ce811d7a09dd96ba543 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -5,13 +5,13 @@
  *      \author: jsola
  */
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/processor/processor_diff_drive.h"
 #include "core/sensor/sensor_diff_drive.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/factor/factor_diff_drive.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 #include "core/frame/frame_base.h"
 #include "core/capture/capture_motion.h"
 #include "core/feature/feature_motion.h"
@@ -114,7 +114,7 @@ class FactorDiffDriveTest : public testing::Test
 {
     public:
         ProblemPtr                  problem;
-        CeresManagerPtr             solver;
+        SolverCeresPtr              solver;
         TrajectoryBasePtr           trajectory;
         ParamsSensorDiffDrivePtr      intr;
         SensorDiffDrivePtr          sensor;
@@ -137,7 +137,7 @@ class FactorDiffDriveTest : public testing::Test
             problem = Problem::create("PO", 2);
             trajectory = problem->getTrajectory();
 
-            solver = std::make_shared<CeresManager>(problem);
+            solver = std::make_shared<SolverCeres>(problem);
 
             // make a sensor first // DO NOT MODIFY THESE VALUES !!!
             intr = std::make_shared<ParamsSensorDiffDrive>();
@@ -160,12 +160,10 @@ class FactorDiffDriveTest : public testing::Test
 
             // frames
             F0 = FrameBase::emplace<FrameBase>(trajectory,
-                                               KEY,
                                                0.0,
                                                "PO",
                                                std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
             F1 = FrameBase::emplace<FrameBase>(trajectory,
-                                               KEY,
                                                1.0,
                                                "PO",
                                                std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
@@ -436,7 +434,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
      */
 
     ProblemPtr problem = Problem::create("PO", 2);
-    CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+    SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
 
     // make a sensor first // DO NOT MODIFY THESE VALUES !!!
     ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>();
@@ -500,7 +498,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
         C->process();
     }
 
-    auto F2 = problem->getLastKeyFrame();
+    auto F2 = problem->getLastFrame();
 
     // Fix boundaries
     F0->fix();
@@ -569,7 +567,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
 
     ProblemPtr problem = Problem::create("PO", 2);
-    CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+    SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
 
     // make a sensor first // DO NOT MODIFY THESE VALUES !!!
     ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>();
@@ -636,7 +634,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
         C->process();
     }
 
-    auto F2 = problem->getLastKeyFrame();
+    auto F2 = problem->getLastFrame();
 
 //    VectorComposite x2c(x2, "PO", {2,1});
     F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index 8c0b11e3b428cfbfa99c91c29bffe53ceeeb1d92..740933a67158634dfd7c6194c8b6a46bd926c063 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -1,10 +1,10 @@
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/factor/factor_odom_2d.h"
 #include "core/capture/capture_odom_2d.h"
 #include "core/math/rotations.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -16,11 +16,11 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity();
 
 // Problem and solver
 ProblemPtr problem_ptr = Problem::create("PO", 2);
-CeresManager ceres_mgr(problem_ptr);
+SolverCeres solver(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero());
 
 // Capture from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
@@ -62,7 +62,7 @@ TEST(FactorOdom2d, fix_0_solve)
         frm1->perturb(5);
 
         // solve for frm1
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
 
@@ -103,7 +103,7 @@ TEST(FactorOdom2d, fix_1_solve)
         frm0->perturb(5);
 
         // solve for frm0
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
 
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp
index 8001bbe70f25f774a55cf0c834950529934bdb36..7a57f0354a19c2a53e8ce6adac68f07713a93664 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -5,12 +5,12 @@
  *      \author: jsola
  */
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/factor/factor_odom_3d.h"
 #include "core/capture/capture_motion.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -34,11 +34,11 @@ Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
 
 // Problem and solver
 ProblemPtr problem_ptr = Problem::create("PO", 3);
-CeresManager ceres_mgr(problem_ptr);
+SolverCeres solver(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), delta);
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
 
 // Capture, feature and factor from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
@@ -66,7 +66,7 @@ TEST(FactorOdom3d, fix_0_solve)
     frm1->setState(x1);
 
     // solve for frm1
-    std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
 
@@ -80,7 +80,7 @@ TEST(FactorOdom3d, fix_1_solve)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6);
 }
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index e3107b7a5f976f5626f22016f64f60695e1fc982..7ecc6ab0848695e7922d9e9fb83f92a73a95e861 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -5,12 +5,12 @@
  *      \author: jsola
  */
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/factor/factor_pose_2d.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/capture/capture_motion.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -27,10 +27,10 @@ Vector3d x0 = pose + Vector3d::Random()*0.25;
 
 // Problem and solver
 ProblemPtr problem = Problem::create("PO", 2);
-CeresManager ceres_mgr(problem);
+SolverCeres solver(problem);
 
 // Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero());
+FrameBasePtr frm0 = problem->emplaceFrame(TimeStamp(0), problem->stateZero());
 
 // Capture, feature and factor from frm1 to frm0
 auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr);
@@ -57,7 +57,7 @@ TEST(FactorPose2d, solve)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = solver.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose, 1e-6);
 
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index d57aee85c27b47e41f9610040c3b8bcceb57ad35..c44374caa2a908080d36e5396a9e41db846b7fb7 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -5,12 +5,12 @@
  *      \author: jsola
  */
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/factor/factor_pose_3d.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/capture/capture_motion.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -33,10 +33,10 @@ Vector7d x0 = pose6toPose7(pose + Vector6d::Random()*0.25);
 
 // Problem and solver
 ProblemPtr problem = Problem::create("PO", 3);
-CeresManager ceres_mgr(problem);
+SolverCeres solver(problem);
 
 // Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() );
+FrameBasePtr frm0 = problem->emplaceFrame(0, problem->stateZero() );
 
 // Capture, feature and factor
 auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr);
@@ -63,7 +63,7 @@ TEST(FactorPose3d, solve)
     frm0->setState(x0);
 
     // solve for frm0
-    std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL);
+    std::string brief_report = solver.solve(SolverManager::ReportVerbosity::FULL);
 
     ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6);
 
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
index c461bbb94743c9ee8b3665650fb00686d979fc20..70cbc464716d50d60bb8ad05cfdb1a4bd6f79d95 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -1,10 +1,10 @@
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/factor/factor_relative_2d_analytic.h"
 #include "core/capture/capture_odom_2d.h"
 #include "core/math/rotations.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -16,11 +16,11 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity();
 
 // Problem and solver
 ProblemPtr problem_ptr = Problem::create("PO", 2);
-CeresManager ceres_mgr(problem_ptr);
+SolverCeres solver(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1.0, Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
 
 // Capture from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
@@ -62,7 +62,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
         frm1->perturb(5);
 
         // solve for frm1
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
 
@@ -103,7 +103,7 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
         frm0->perturb(5);
 
         // solve for frm0
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
 
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index b9ad5bbe208d981a4ca69aa3eab8eac806f62e14..f6e8e18f10247e1c819121bc9e4494dd26ac6efe 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -1,3 +1,4 @@
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
@@ -6,7 +7,6 @@
 #include "core/processor/processor_odom_2d.h"
 #include "core/math/rotations.h"
 
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -20,15 +20,15 @@ Matrix3d data_cov = 0.1*Matrix3d::Identity();
 
 // Problem and solver
 ProblemPtr problem_ptr = Problem::create("PO", 2);
-CeresManager ceres_mgr(problem_ptr);
+SolverCeres solver(problem_ptr);
 
 // Sensor
 auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
 auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, Vector3d::Zero() );
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1, Vector3d::Zero() );
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
 
 // Capture from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
@@ -40,7 +40,7 @@ TEST(FactorRelativePose2dWithExtrinsics, check_tree)
 
 TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
 {
-    for (int i = 0; i < 1e3; i++)
+    for (int i = 0; i < 1e2; i++)
     {
         // Random delta
         Vector3d delta = Vector3d::Random() * 10;
@@ -77,7 +77,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
         frm1->perturb(5);
 
         // solve for frm1
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
 
@@ -88,7 +88,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
 
 TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
 {
-    for (int i = 0; i < 1e3; i++)
+    for (int i = 0; i < 1e2; i++)
     {
         // Random delta
         Vector3d delta = Vector3d::Random() * 10;
@@ -125,7 +125,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
         frm0->perturb(5);
 
         // solve for frm0
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
 
@@ -136,7 +136,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
 
 TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
 {
-    for (int i = 0; i < 1e3; i++)
+    for (int i = 0; i < 1e2; i++)
     {
         // Random delta
         Vector3d delta = Vector3d::Random() * 10;
@@ -173,7 +173,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
         sensor_odom2d->getP()->perturb(1);
 
         // solve for frm0
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
 
@@ -184,7 +184,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
 
 TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
 {
-    for (int i = 0; i < 1e3; i++)
+    for (int i = 0; i < 1e2; i++)
     {
         // Random delta
         Vector3d delta = Vector3d::Random() * 10;
@@ -223,7 +223,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
         //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
 
         // solve for frm0
-        std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 
         ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
 
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index abdefc83d7d4a3e1f041f5355b3139c1276775cb..ac405e6fa1e9f4bcdc7976d8f0adc2014cb79c5f 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -22,7 +22,7 @@ using namespace wolf;
 
 TEST(FrameBase, GettersAndSetters)
 {
-    FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
 
     // getters
     ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -31,13 +31,11 @@ TEST(FrameBase, GettersAndSetters)
     F->getTimeStamp(t);
     ASSERT_EQ(t, 1);
     ASSERT_FALSE(F->isFixed());
-    ASSERT_EQ(F->isKey(), false);
-    ASSERT_EQ(F->isKeyOrAux(), false);
 }
 
 TEST(FrameBase, StateBlocks)
 {
-    FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
 
     ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
     ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -47,12 +45,10 @@ TEST(FrameBase, StateBlocks)
 
 TEST(FrameBase, LinksBasic)
 {
-    FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
 
     ASSERT_FALSE(F->getTrajectory());
     ASSERT_FALSE(F->getProblem());
-    //    ASSERT_THROW(f->getPreviousFrame(), std::runtime_error);  // protected by assert()
-    //    ASSERT_EQ(f->getStatus(), ST_ESTIMATED);                  // protected
     SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d());
     ASSERT_FALSE(F->getCaptureOf(S));
     ASSERT_TRUE(F->getCaptureList().empty());
@@ -69,8 +65,8 @@ TEST(FrameBase, LinksToTree)
     intrinsics_odo.k_disp_to_disp = 1;
     intrinsics_odo.k_rot_to_rot = 1;
     auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
-    auto F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-    auto F2 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
     auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
     WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
@@ -115,11 +111,6 @@ TEST(FrameBase, LinksToTree)
     ASSERT_FALSE(F1->isFixed());
     F1->getO()->fix();
     ASSERT_TRUE(F1->isFixed());
-
-    // set key
-    F1->setKey();
-    ASSERT_TRUE(F1->isKey());
-    ASSERT_TRUE(F1->isKeyOrAux());
 }
 
 #include "core/state_block/state_quaternion.h"
@@ -131,7 +122,7 @@ TEST(FrameBase, GetSetState)
     StateQuaternionPtr sbq = make_shared<StateQuaternion>();
 
     // Create frame
-    FrameBase F(NON_ESTIMATED, 1, sbp, sbq, sbv);
+    FrameBase F(1, sbp, sbq, sbv);
 
     // Give values to vectors and vector blocks
     VectorXd x(10), x1(10), x2(10);
@@ -155,28 +146,24 @@ TEST(FrameBase, GetSetState)
 
 TEST(FrameBase, Constructor_composite)
 {
-    FrameBase F = FrameBase(KEY,
-                            0.0,
+    FrameBase F = FrameBase(0.0,
                             "POV",
                             VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
 
-    ASSERT_TRUE         (F.isKey());
     ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
     ASSERT_TRUE         (F.getO()->hasLocalParametrization());
-    ASSERT_EQ           (F.getStateBlock("V")->getSize(), 3);
+    ASSERT_EQ           (F.getStateBlock('V')->getSize(), 3);
 }
 
 TEST(FrameBase, Constructor_vectors)
 {
-    FrameBase F = FrameBase(KEY,
-                            0.0,
+    FrameBase F = FrameBase(0.0,
                             "POV",
                             {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
 
-    ASSERT_TRUE         (F.isKey());
     ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
     ASSERT_TRUE         (F.getO()->hasLocalParametrization());
-    ASSERT_EQ           (F.getStateBlock("V")->getSize(), 3);
+    ASSERT_EQ           (F.getStateBlock('V')->getSize(), 3);
 }
 
 
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index 1369fd5a87dc31f4d0a7af14a929f0d23637b041..0d42d813257b1b2b26aeefe3fb908a3b29b5b616 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -6,13 +6,13 @@
  */
 
 
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 #include "core/utils/utils_gtest.h"
 
 #include "core/frame/frame_base.h"
 #include "core/sensor/sensor_base.h"
 #include "core/landmark/landmark_base.h"
 #include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace wolf;
 
@@ -41,8 +41,8 @@ class HasStateBlocksTest : public testing::Test
             sbo1 = std::make_shared<StateQuaternion>();
             sbv1 = std::make_shared<StateBlock>(3); // size 3
 
-            F0 = std::make_shared<FrameBase>(NON_ESTIMATED, 0.0, sbp0, sbo0); // non KF
-            F1 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.0, nullptr);    // non KF
+            F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+            F1 = std::make_shared<FrameBase>(1.0, nullptr);    // non KF
 
         }
         void TearDown() override{}
@@ -50,40 +50,25 @@ class HasStateBlocksTest : public testing::Test
 };
 
 
-TEST_F(HasStateBlocksTest, Notifications_setKey_add)
-{
-    Notification n;
-    ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
-    F0->link(problem->getTrajectory());
-
-    ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
-    F0->setKey();
-
-    ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
-    ASSERT_EQ(n, ADD);
-}
-
 TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
 {
     Notification n;
 
-    // First add SB, than make KF
+    // First add SB, then make KF
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
 
-    F0->link(problem->getTrajectory());
+    // F0->link(problem->getTrajectory());
 
-    ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+    // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
 
-    ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+    // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
 
-    F0->addStateBlock("V", sbv0);
+    F0->addStateBlock('V', sbv0);
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
 
-    F0->setKey();
+    F0->link(problem);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
     ASSERT_EQ(n, ADD);
@@ -99,10 +84,10 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
 
     // first make KF, then add SB
 
-    F1->link(problem->getTrajectory());
-    F1->setKey();
+    // F1->link(problem->getTrajectory());
+    F1->link(problem);
 
-    F1->addStateBlock("P", sbp1);
+    F1->addStateBlock('P', sbp1);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
     ASSERT_EQ(n, ADD);
@@ -111,7 +96,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
 
-    F1->addStateBlock("V", sbv1);
+    F1->addStateBlock('V', sbv1);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
     ASSERT_EQ(n, ADD);
@@ -121,7 +106,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
 TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
 {
 
-    CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+    SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
 
 
     Notification n;
@@ -130,9 +115,8 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
 
-    F0->link(problem->getTrajectory());
-    F0->addStateBlock("V", sbv0);
-    F0->setKey();
+    F0->addStateBlock('V', sbv0);
+    F0->link(problem);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
     ASSERT_EQ(n, ADD);
@@ -158,7 +142,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
 
     // Add again the same SB. This should crash
 
-    ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" );
+    ASSERT_DEATH( F0->addStateBlock('V', sbv0) , "" );
 
 }
 
@@ -170,51 +154,48 @@ TEST_F(HasStateBlocksTest, hasStateBlock)
 
 TEST_F(HasStateBlocksTest, stateBlockKey)
 {
-    std::string key;
+    char key;
     ASSERT_TRUE(F0->stateBlockKey(sbp0, key));
-    ASSERT_TRUE(key == "P");
+    ASSERT_TRUE(key == 'P');
 
     ASSERT_FALSE(F0->stateBlockKey(sbp1, key));
-    ASSERT_TRUE(key == "");
+    ASSERT_TRUE(key == '0');
 }
 
 TEST_F(HasStateBlocksTest, getState_structure)
 {
-    F0->addStateBlock("V", sbv0); // now KF0 is POV
+    F0->addStateBlock('V', sbv0); // now KF0 is POV
 
     WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());
 
     auto state0 = F0->getState();
     WOLF_DEBUG("getState()     = ", state0);
     ASSERT_EQ(state0.size(), 3);
-    ASSERT_TRUE(state0.count("P"));
-    ASSERT_TRUE(state0.count("O"));
-    ASSERT_TRUE(state0.count("V"));
+    ASSERT_TRUE(state0.count('P'));
+    ASSERT_TRUE(state0.count('O'));
+    ASSERT_TRUE(state0.count('V'));
 
     state0 = F0->getState("PO");
     WOLF_DEBUG("getState(\"PO\") = ", state0);
     ASSERT_EQ(state0.size(), 2);
-    ASSERT_TRUE(state0.count("P"));
-    ASSERT_TRUE(state0.count("O"));
-    ASSERT_FALSE(state0.count("V"));
+    ASSERT_TRUE(state0.count('P'));
+    ASSERT_TRUE(state0.count('O'));
+    ASSERT_FALSE(state0.count('V'));
 
     state0 = F0->getState("PV");
     WOLF_DEBUG("getState(\"PV\") = ", state0);
     ASSERT_EQ(state0.size(), 2);
-    ASSERT_TRUE(state0.count("P"));
-    ASSERT_FALSE(state0.count("O"));
-    ASSERT_TRUE(state0.count("V"));
+    ASSERT_TRUE(state0.count('P'));
+    ASSERT_FALSE(state0.count('O'));
+    ASSERT_TRUE(state0.count('V'));
 
     state0 = F0->getState("OW"); // W does not exist
     WOLF_DEBUG("getState(\"OW\") = ", state0);
     ASSERT_EQ(state0.size(), 1);
-    ASSERT_FALSE(state0.count("P"));
-    ASSERT_TRUE(state0.count("O"));
-    ASSERT_FALSE(state0.count("V"));
-    ASSERT_FALSE(state0.count("W"));
-
-
-
+    ASSERT_FALSE(state0.count('P'));
+    ASSERT_TRUE(state0.count('O'));
+    ASSERT_FALSE(state0.count('V'));
+    ASSERT_FALSE(state0.count('W'));
 
 }
 
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 8d97fc8d063ed1a08c36b25f259353cc74852f76..ab545f2eca3122493a20ac31024faab520f46c64 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -16,7 +16,6 @@
 // Wolf includes
 #include "core/state_block/state_block.h"
 #include "core/common/wolf.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 #include "core/capture/capture_pose.h"
 
 // STL includes
@@ -28,6 +27,7 @@
 // General includes
 #include <iostream>
 #include <iomanip>      // std::setprecision
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 
 using namespace wolf;
 using namespace Eigen;
@@ -74,29 +74,26 @@ void show(const ProblemPtr& problem)
     using std::endl;
     cout << std::setprecision(4);
 
-    for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
+    for (FrameBasePtr F : *problem->getTrajectory())
     {
-        if (F->isKey())
+        cout << "----- Key Frame " << F->id() << " -----" << endl;
+        if (!F->getCaptureList().empty())
         {
-            cout << "----- Key Frame " << F->id() << " -----" << endl;
-            if (!F->getCaptureList().empty())
+            auto C = F->getCaptureList().front();
+            if (!C->getFeatureList().empty())
             {
-                auto C = F->getCaptureList().front();
-                if (!C->getFeatureList().empty())
-                {
-                    auto f = C->getFeatureList().front();
-                    cout << "  feature measure: \n"
-                            << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
-                            << endl;
-                    cout << "  feature covariance: \n"
-                            << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
-                }
+                auto f = C->getFeatureList().front();
+                cout << "  feature measure: \n"
+                        << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
+                        << endl;
+                cout << "  feature covariance: \n"
+                        << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
             }
-            cout << "  state: \n" << F->getStateVector().transpose() << endl;
-            Eigen::MatrixXd cov;
-            problem->getFrameCovariance(F,cov);
-            cout << "  covariance: \n" << cov << endl;
         }
+        cout << "  state: \n" << F->getStateVector().transpose() << endl;
+        Eigen::MatrixXd cov;
+        problem->getFrameCovariance(F,cov);
+        cout << "  covariance: \n" << cov << endl;
     }
 }
 
@@ -125,21 +122,21 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
     Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
 
     ProblemPtr          Pr = Problem::create("PO", 2);
-    CeresManager        ceres_manager(Pr);
+    SolverCeres         solver(Pr);
 
     // KF0 and absolute prior
     FrameBasePtr        F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
 
     // KF1 and motion from KF0
     t += dt;
-    FrameBasePtr        F1 = Pr->emplaceFrame(KEY, t, Vector3d::Zero());
+    FrameBasePtr        F1 = Pr->emplaceFrame(t, Vector3d::Zero());
     auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
     auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
     auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
 
     // KF2 and motion from KF1
     t += dt;
-    FrameBasePtr        F2 = Pr->emplaceFrame(KEY, t, Vector3d::Zero());
+    FrameBasePtr        F2 = Pr->emplaceFrame(t, Vector3d::Zero());
     auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
     auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
     auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
@@ -150,10 +147,10 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
     F0->setState(Vector3d(1,2,3));
     F1->setState(Vector3d(2,3,1));
     F2->setState(Vector3d(3,1,2));
-    std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
+    std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
 //    std::cout << report << std::endl;
 
-    ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 //    show(Pr);
 
     Matrix3d P1, P2;
@@ -218,17 +215,17 @@ TEST(Odom2d, VoteForKfAndSolve)
     //       KF - * -- * -- KF - * -- * -- KF - *
 
     // Ceres wrapper
-    CeresManager ceres_manager(problem);
+    SolverCeres solver(problem);
 
     // Origin Key Frame
     auto KF = problem->setPriorFactor(x0, s0, t, dt/2);
     processor_odom2d->setOrigin(KF);
 
-    ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-    ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    solver.solve(SolverManager::ReportVerbosity::BRIEF);
+    solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
     //    std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
-    //    std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl;
+    //    std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl;
     //    std::cout << "Motion data  : " << data.transpose() << std::endl;
 
     // Check covariance values
@@ -297,8 +294,8 @@ TEST(Odom2d, VoteForKfAndSolve)
     }
 
     // Solve
-    std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-    ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+    solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
 
     problem->print(4,1,1,1);
@@ -306,10 +303,8 @@ TEST(Odom2d, VoteForKfAndSolve)
     // Check the 3 KFs' states and covariances
     MatrixXd computed_cov;
     int n = 0;
-    for (auto F : problem->getTrajectory()->getFrameList())
+    for (auto F : *problem->getTrajectory())
     {
-        if (!F->isKey()) break;
-
         ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n]   , 1e-6);
         ASSERT_TRUE         (F->getCovariance(computed_cov));
         ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n]    , 1e-6);
@@ -358,7 +353,7 @@ TEST(Odom2d, KF_callback)
     processor_odom2d->setTimeTolerance(dt/2);
 
     // Ceres wrapper
-    CeresManager ceres_manager(problem);
+    SolverCeres solver(problem);
 
     // Origin Key Frame
     FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
@@ -423,12 +418,12 @@ TEST(Odom2d, KF_callback)
     std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
 
     Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
-    FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, t_split, x_split);
+    FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
 
     // there must be 2KF and one F
-    ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
+    ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
     // The last KF must have TS = 0.08
-    ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000);
+    ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
 
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
@@ -442,13 +437,13 @@ TEST(Odom2d, KF_callback)
 
     MotionBuffer key_buffer_n = key_capture_n->getBuffer();
 
-    std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
 //    std::cout << report << std::endl;
-    ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
-    ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
+    ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
     MatrixXd computed_cov;
-    ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+    ASSERT_TRUE(problem->getLastFrameCovariance(computed_cov));
     ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
 
     ////////////////////////////////////////////////////////////////
@@ -460,7 +455,7 @@ TEST(Odom2d, KF_callback)
     problem->print(4,1,1,1);
 
     x_split = processor_odom2d->getState(t_split).vector("PO");
-    FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, t_split, x_split);
+    FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
 
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
@@ -479,8 +474,8 @@ TEST(Odom2d, KF_callback)
     keyframe_1->setState(Vector3d(2,3,1));
     keyframe_2->setState(Vector3d(3,1,2));
 
-    report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-    ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+    solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
     problem->print(4,1,1,1);
 
@@ -493,7 +488,7 @@ TEST(Odom2d, KF_callback)
     // check other KF in the future of the split KF
     MatrixXd KF2_cov;
     ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
-    ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
+    ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
     ASSERT_MATRIX_APPROX(KF2_cov,                                integrated_cov_vector [n_split], 1e-6);
 
     // Check full trajectory
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 056fbcf0bb231962e109ea60f404a5a0cefd58f5..d1fce41e50c72c91fb520d748a8154d5d516a1e6 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -34,10 +34,10 @@ class BufferPackKeyFrameTest : public testing::Test
 
         void SetUp(void) override
         {
-            f10 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(10),nullptr,nullptr,nullptr);
-            f20 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(20),nullptr,nullptr,nullptr);
-            f21 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(21),nullptr,nullptr,nullptr);
-            f28 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(28),nullptr,nullptr,nullptr);
+            f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
+            f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
+            f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
+            f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
 
             tt10 = 1.0;
             tt20 = 1.0;
@@ -230,7 +230,7 @@ TEST_F(BufferPackKeyFrameTest, removeUpTo)
     // Specifically, only f28 should remain
     pack_kf_buffer.add(f28, tt28);
     ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
-    FrameBasePtr f22 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(22),nullptr,nullptr,nullptr);
+    FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
     PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
     pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
     ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index a512601e2b75ed7035f3b70745294ccffe1a9596..8faac803c5ffe7e5780557fbc08155e43c3ca587 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -9,15 +9,15 @@
 
 
 #include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 #include "core/sensor/sensor_odom_3d.h"
 
 #include <iostream>
+#include "../include/core/ceres_wrapper/solver_ceres.h"
 
 using namespace wolf;
 
 ProblemPtr problem_ptr = Problem::create("PO", 3);
-CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
+SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
 Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).finished());
 Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
 Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
@@ -37,7 +37,7 @@ TEST(ParameterPrior, prior_p)
 {
     odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3d::Identity());
 
-    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
 }
@@ -46,7 +46,7 @@ TEST(ParameterPrior, prior_o)
 {
     odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3d::Identity());
 
-    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
 }
@@ -55,7 +55,7 @@ TEST(ParameterPrior, prior_p_overwrite)
 {
     odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3d::Identity());
 
-    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
 }
@@ -64,7 +64,7 @@ TEST(ParameterPrior, prior_p_segment)
 {
     odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2);
 
-    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
 }
@@ -72,7 +72,7 @@ TEST(ParameterPrior, prior_p_segment)
 TEST(ParameterPrior, prior_o_segment)
 {
     // constraining segment of quaternion is not allowed
-    ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter("O", prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
+    ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter('O', prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
 }
 
 
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 1470052b849465705eb860b34600d8d624dfb61c..181fb1ce43bc80a8a04034191973d5ac60635b1e 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -113,7 +113,7 @@ TEST(Problem, SetOrigin_PO_2d)
     VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
 
     P->setPriorFactor(x0, s0, t0, 1.0);
-
+    WOLF_INFO("printing.-..");
     P->print(4,1,1,1);
 
     // check that no sensor has been added
@@ -132,7 +132,7 @@ TEST(Problem, SetOrigin_PO_2d)
     F->getFactorList(fac_list);
 
     // check that we have one frame (prior)
-    ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+    ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
     // check that we have one capture (prior)
     ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
     // check that we have two features (prior: one per state block)
@@ -191,7 +191,7 @@ TEST(Problem, SetOrigin_PO_3d)
     F->getFactorList(fac_list);
 
     // check that we have one frame (prior)
-    ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+    ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
     // check that we have one capture (prior)
     ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
     // check that we have two features (prior: one per state block)
@@ -224,12 +224,13 @@ TEST(Problem, emplaceFrame_factory)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    FrameBasePtr f0 = P->emplaceFrame(KEY, 0, "PO" , 2,  VectorXd(3)  );
-    FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3,  VectorXd(7)  );
-    FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3,  VectorXd(10) );
+    FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2,  VectorXd(3)  );
+    FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3,  VectorXd(7)  );
+    FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3,  VectorXd(10) );
+
 
     // check that all frames are effectively in the trajectory
-    ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
+    ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
 
     ASSERT_EQ(f0->getStateVector().size(), 3 );
     ASSERT_EQ(f1->getStateVector().size(), 7 );
@@ -239,6 +240,8 @@ TEST(Problem, emplaceFrame_factory)
     ASSERT_EQ(f0->getProblem(), P);
     ASSERT_EQ(f1->getProblem(), P);
     ASSERT_EQ(f2->getProblem(), P);
+
+    ASSERT_EQ(P->getFrameStructure(), "POV");
 }
 
 TEST(Problem, StateBlocks)
@@ -246,7 +249,7 @@ TEST(Problem, StateBlocks)
     std::string wolf_root = _WOLF_ROOT_DIR;
 
     ProblemPtr P = Problem::create("PO", 3);
-    Eigen::Vector7d xs3d;
+    Eigen::Vector7d xs3d; xs3d << 1,2,3,0,0,0,1; // required normalized quaternion (solver manager checks this)
     Eigen::Vector3d xs2d;
 
     // 2 state blocks, fixed
@@ -261,7 +264,7 @@ TEST(Problem, StateBlocks)
     auto pm = P->installProcessor("ProcessorOdom3d",                "odom integrator",  "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
 
     // 2 state blocks, estimated
-    auto KF = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
+    auto KF = P->emplaceFrame(0, "PO", 3, xs3d );
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
 
     // Notifications
@@ -320,7 +323,7 @@ TEST(Problem, Covariances)
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
-    FrameBasePtr F = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
+    FrameBasePtr F = P->emplaceFrame(0, "PO", 3, xs3d );
 
     // set covariance (they are not computed without a solver)
     P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -349,14 +352,14 @@ TEST(Problem, perturb)
     intr->ticks_per_wheel_revolution    = 100;
     Vector3d extr(0,0,0);
     auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
-    sensor->setStateBlockDynamic("I", true);
+    sensor->setStateBlockDynamic('I', true);
 
     Vector3d pose; pose << 0,0,0;
 
     int i = 0;
     for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
     {
-        auto F = problem->emplaceFrame(KEY, t, pose );
+        auto F = problem->emplaceFrame(t, pose );
         if (i==0) F->fix();
 
         for (int j = 0; j< 2 ; j++)
@@ -385,21 +388,22 @@ TEST(Problem, perturb)
 
     //// CHECK ALL STATE BLOCKS ///
 
-    double prec = 1e-2;
+#define prec 1e-2
 
     for (auto S : problem->getHardware()->getSensorList())
     {
-        ASSERT_TRUE (S->getP()->getState().isApprox(Vector2d(0,0), prec) );
-        ASSERT_TRUE (S->getO()->getState().isApprox(Vector1d(0), prec) );
+        ASSERT_MATRIX_APPROX(S->getP()->getState(), Vector2d(0,0), prec );
+        ASSERT_MATRIX_APPROX(S->getO()->getState(), Vector1d(0), prec );
         ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
     }
 
-    for (auto F : problem->getTrajectory()->getFrameList())
+    for (auto pair_T_F : problem->getTrajectory()->getFrameMap())
     {
+        auto F = pair_T_F.second;
         if (F->isFixed()) // fixed
         {
-            ASSERT_TRUE (F->getP()->getState().isApprox(Vector2d(0,0), prec) );
-            ASSERT_TRUE (F->getO()->getState().isApprox(Vector1d(0), prec) );
+            ASSERT_MATRIX_APPROX (F->getP()->getState(), Vector2d(0,0), prec );
+            ASSERT_MATRIX_APPROX (F->getO()->getState(), Vector1d(0), prec );
         }
         else
         {
@@ -416,8 +420,8 @@ TEST(Problem, perturb)
     {
         if ( L->isFixed() ) // fixed
         {
-            ASSERT_TRUE (L->getP()->getState().isApprox(Vector2d(1,2), prec) );
-            ASSERT_TRUE (L->getO()->getState().isApprox(Vector1d(3), prec) );
+            ASSERT_MATRIX_APPROX (L->getP()->getState(), Vector2d(1,2), prec );
+            ASSERT_MATRIX_APPROX (L->getO()->getState(), Vector1d(3), prec );
         }
         else
         {
@@ -439,14 +443,14 @@ TEST(Problem, check)
     intr->ticks_per_wheel_revolution    = 100;
     Vector3d extr(0,0,0);
     auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
-    sensor->setStateBlockDynamic("I", true);
+    sensor->setStateBlockDynamic('I', true);
 
     Vector3d pose; pose << 0,0,0;
 
     int i = 0;
     for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
     {
-        auto F = problem->emplaceFrame(KEY, t, pose);
+        auto F = problem->emplaceFrame(t, pose);
         if (i==0) F->fix();
 
         for (int j = 0; j< 2 ; j++)
@@ -477,7 +481,7 @@ TEST(Problem, check)
 
     // remove stuff from problem, and re-check
 
-    auto F = problem->getLastKeyFrame();
+    auto F = problem->getLastFrame();
 
     auto cby = F->getConstrainedByList().front();
 
@@ -554,6 +558,9 @@ TEST(Problem, getState)
     WOLF_DEBUG("P () = ", P->getState("P"));
     WOLF_DEBUG("PO() = ", P->getState("PO"));
     WOLF_DEBUG("x () = ", P->getState());
+    ASSERT_EQ(P->getState("P").size(), 1);
+    ASSERT_EQ(P->getState("PO").size(), 2);
+    ASSERT_EQ(P->getState().size(), 2);
 
 }
 
@@ -561,6 +568,6 @@ TEST(Problem, getState)
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  ::testing::GTEST_FLAG(filter) = "Problem.getState";
+//  ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
   return RUN_ALL_TESTS();
 }
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index ed01245b29c903a7584e932196fb93a2c01c5c20..c3277078df9300439a6dee3721f0084df9751522 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -117,12 +117,16 @@ TEST(ProcessorBase, KeyFrameCallback)
 
         capt_odo->setTimeStamp(t);
         std::cout << "2\n";
+//        auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
+//        auto last_ptr = proc_odo_motion->last_ptr_;
+//        auto last_ptr_frame = last_ptr->getFrame();
         proc_odo->captureCallback(capt_odo);
         std::cout << "3\n";
 
         // Track
         capt_trk = make_shared<CaptureVoid>(t, sens_trk);
         std::cout << "4\n";
+        problem->print(4,1,1,1, std::cout);
         proc_trk->captureCallback(capt_trk);
         std::cout << "5\n";
 
@@ -130,7 +134,7 @@ TEST(ProcessorBase, KeyFrameCallback)
         std::cout << "6\n";
 
         // Only odom creating KFs
-        ASSERT_TRUE( problem->getLastKeyFrame()->getStructure().compare("PO")==0 );
+        ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 );
     }
 }
 
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index b13061e657badbd2ed81e9ea82b0f6078f3803f0..fc30487fe96765f36caea276defddc7abee9a6ac 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -352,9 +352,11 @@ TEST_F(ProcessorDiffDriveTest, linear)
     data(0) = 100.0 ; // one turn of the wheels
     data(1) = 100.0 ;
 
+    t += 1.0;
     auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     C->process();
+
     WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
 
     // radius is 1.0m, 100 ticks per revolution, so advanced distance is
@@ -377,6 +379,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
     processor->setOrigin(F0);
 
     // Straight one turn of the wheels, in one go
+    t += 1.0;
     data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
     data(1) =  20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
 
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 90d55137f88921c61c2e79bee1ea19d0f9047111..4e50c870348da1877b3a599c48158730e2df47ab 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -34,11 +34,10 @@ protected:
     bool detectFeatures(CaptureBasePtr cap) override     { return true;};
     CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
     {
-        for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList())
-            if (kf->isKey())
-                for (CaptureBasePtr cap : kf->getCaptureList())
-                    if (cap != _capture)
-                        return cap;
+        for (FrameBasePtr kf : *getProblem()->getTrajectory())
+            for (CaptureBasePtr cap : kf->getCaptureList())
+                if (cap != _capture)
+                    return cap;
         return nullptr;
     };
     void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
@@ -87,7 +86,7 @@ TEST(ProcessorLoopClosure, installProcessor)
 
     // new KF
     t += dt;
-    auto kf = problem->emplaceFrame(KEY, t, x); //KF2
+    auto kf = problem->emplaceFrame(t, x); //KF2
     // emplace a capture in KF
     auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
     proc_lc->captureCallback(capt_lc);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 410ebdf1faea8175c8e73435670fdb46de85fd9e..e531defd5721e3374308d67e793a01f682701577 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -12,7 +12,6 @@
 
 #include "core/sensor/sensor_odom_2d.h"
 #include "core/processor/processor_odom_2d.h"
-#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -51,11 +50,6 @@ class ProcessorMotion_test : public testing::Test{
         Vector2d                    data;
         Matrix2d                    data_cov;
 
-//        ProcessorMotion_test() :
-//            ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
-//            dt(0)
-//        { }
-
         void SetUp() override
         {
             std::string wolf_root = _WOLF_ROOT_DIR;
@@ -64,7 +58,7 @@ class ProcessorMotion_test : public testing::Test{
             problem = Problem::create("PO", 2);
             sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
             ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
-            params->time_tolerance  = 0.25;
+            params->time_tolerance  = 0.5;
             params->dist_traveled   = 100;
             params->angle_turned    = 6.28;
             params->max_time_span   = 2.5*dt; // force KF at every third process()
@@ -121,12 +115,12 @@ TEST_F(ProcessorMotion_test, getState_structure)
         WOLF_DEBUG("t: ", t, "  x: ", problem->getState().vector("PO").transpose());
     }
 
-    ASSERT_TRUE (processor->getState("P").count("P"));
-    ASSERT_FALSE(processor->getState("P").count("O"));
-    ASSERT_FALSE(processor->getState("O").count("P"));
-    ASSERT_TRUE (processor->getState("O").count("O"));
+    ASSERT_TRUE (processor->getState("P").count('P'));
+    ASSERT_FALSE(processor->getState("P").count('O'));
+    ASSERT_FALSE(processor->getState("O").count('P'));
+    ASSERT_TRUE (processor->getState("O").count('O'));
 
-    WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState("V"));
+    WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState('V'));
     ASSERT_EQ   (processor->getState("V").size(), 0);
 }
 
@@ -147,14 +141,16 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
         capture->setTimeStamp(t);
         capture->setData(data);
         capture->setDataCovariance(data_cov);
-        processor->captureCallback(capture);
+        capture->process();
         WOLF_DEBUG("t: ", t, "  x: ", problem->getState().vector("PO").transpose());
     }
 
-    ASSERT_TRUE (processor->getState(7, "P").count("P"));
-    ASSERT_FALSE(processor->getState(7, "P").count("O"));
-    ASSERT_FALSE(processor->getState(7, "O").count("P"));
-    ASSERT_TRUE (processor->getState(7, "O").count("O"));
+    problem->print(2,1,1,1);
+
+    ASSERT_TRUE (processor->getState(7, "P").count('P'));
+    ASSERT_FALSE(processor->getState(7, "P").count('O'));
+    ASSERT_FALSE(processor->getState(7, "O").count('P'));
+    ASSERT_TRUE (processor->getState(7, "O").count('O'));
 
     WOLF_DEBUG("processor->getState(7, \"V\") = ", processor->getState(7, "V"));
     ASSERT_EQ   (processor->getState(7, "V").size(), 0);
@@ -293,6 +289,83 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
     ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
 }
 
+TEST_F(ProcessorMotion_test, mergeCaptures)
+{
+    // Prior
+    Vector3d x0; x0 << 0, 0, 0;
+    Matrix3d P0; P0.setIdentity();
+
+    data << 1, 2*M_PI/10; // advance in circle
+    data_cov.setIdentity();
+    TimeStamp t(0.0);
+
+    for (int i = 0; i<10; i++) // one full turn exactly
+    {
+        t += dt;
+        capture->setTimeStamp(t);
+        capture->setData(data);
+        capture->setDataCovariance(data_cov);
+        processor->captureCallback(capture);
+        WOLF_DEBUG("t: ", t, "  x: ", problem->getState().vector("PO").transpose());
+    }
+
+    SensorBasePtr    S = processor->getSensor();
+
+    TimeStamp        t_target = 8.5;
+    FrameBasePtr     F_target = problem->emplaceFrame(t_target);
+    CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+    CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+                                                                    "ODOM 2d",
+                                                                    t_target,
+                                                                    sensor,
+                                                                    data,
+                                                                    nullptr);
+
+    // copy initial buffer
+    MotionBuffer initial_buffer = C_source->getBuffer();
+
+    processor->splitBuffer(C_source,
+                           t_target,
+                           F_target,
+                           C_target);
+
+    C_target->getBuffer().print(1,1,1,0);
+    C_source->getBuffer().print(1,1,1,0);
+
+    // merge captures
+    processor->mergeCaptures(C_target, C_source);
+
+    C_target->getBuffer().print(1,1,1,0);
+    C_source->getBuffer().print(1,1,1,0);
+
+    // Check that merged buffer is the initial one (before splitting)
+    EXPECT_EQ(C_source->getBuffer().size(), initial_buffer.size());
+
+    auto init_it = initial_buffer.begin();
+    auto res_it = C_source->getBuffer().begin();
+    while (init_it != initial_buffer.end())
+    {
+        EXPECT_EQ(init_it->data_size_, init_it->data_size_);
+        EXPECT_EQ(init_it->delta_size_, init_it->delta_size_);
+        EXPECT_EQ(init_it->cov_size_, init_it->cov_size_);
+        EXPECT_EQ(init_it->calib_size_, init_it->calib_size_);
+        EXPECT_EQ(init_it->ts_, init_it->ts_);
+
+        EXPECT_MATRIX_APPROX(init_it->data_ ,init_it->data_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->data_cov_,init_it->data_cov_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->delta_, init_it->delta_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->delta_cov_,  init_it->delta_cov_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->delta_integr_, init_it->delta_integr_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->delta_integr_cov_, init_it->delta_integr_cov_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->jacobian_delta_, init_it->jacobian_delta_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->jacobian_delta_integr_, init_it->jacobian_delta_integr_, 1e-12);
+        EXPECT_MATRIX_APPROX(init_it->jacobian_calib_, init_it->jacobian_calib_, 1e-12);
+
+        init_it++;
+        res_it++;
+    }
+}
+
 TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
 {
     // Prior
@@ -316,7 +389,7 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
     SensorBasePtr    S = processor->getSensor();
 
     TimeStamp        t_target = 8.5;
-    FrameBasePtr     F_target = problem->emplaceFrame(KEY, t_target);
+    FrameBasePtr     F_target = problem->emplaceFrame(t_target);
     CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
     CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
                                                                     "ODOM 2d",
@@ -361,7 +434,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
     SensorBasePtr    S = processor->getSensor();
 
     TimeStamp        t_target = 8.5;
-    FrameBasePtr     F_target = problem->emplaceFrame(KEY, t_target);
+    FrameBasePtr     F_target = problem->emplaceFrame(t_target);
     CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
     CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
                                                                     "ODOM 2d",
@@ -406,7 +479,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
     SensorBasePtr    S = processor->getSensor();
 
     TimeStamp        t_target = 8.5;
-    FrameBasePtr     F_target = problem->emplaceFrame(KEY, t_target);
+    FrameBasePtr     F_target = problem->emplaceFrame(t_target);
     CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
     CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
                                                                     "ODOM 2d",
@@ -424,6 +497,37 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
     C_source->getBuffer().print(1,1,1,0);
 }
 
+TEST_F(ProcessorMotion_test, initOdometry)
+{
+    auto odometry = processor->getOdometry();
+
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+}
+
+TEST_F(ProcessorMotion_test, integrateOdometry)
+{
+    auto odometry = processor->getOdometry();
+
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+
+    data << 1,0;
+    capture->setData(data);
+
+    capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+    capture->process();
+    odometry = processor->getOdometry();
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+
+    capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+    capture->process();
+    odometry = processor->getOdometry();
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+}
+
 
 int main(int argc, char **argv)
 {
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 6238a5786533fc89df682364305db55661921e92..49593d64afa531c28a8a490de0ba562ed1c13bb2 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -287,8 +287,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
     CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
     cap1->process();
 
-    ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
-    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id());
+    ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+    ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap1->getFrame()->id());
     ASSERT_TRUE(problem->check(0));
 
     //2ND TIME
@@ -321,8 +321,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
     CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
     cap5->process();
 
-    ASSERT_TRUE(cap4->getFrame()->isKey());
-    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id());
+    ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id());
     ASSERT_TRUE(problem->check(0));
 
     // check factors
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 44dfbdf14cc1f53ea4962cc7f2517d37ef5210b2..d0fe1b1d8a748df603db674cf7604720c69e4d71 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -321,8 +321,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
     CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
     cap1->process();
 
-    ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
-    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame());
+    ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+    ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap1->getFrame());
 
     //2ND TIME
     WOLF_DEBUG("Second time...");
@@ -351,7 +351,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
     CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
     cap5->process();
 
-    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame());
+    ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap4->getFrame());
     ASSERT_EQ(processor->getOrigin(), cap4);
     ASSERT_EQ(processor->getLast(), cap5);
 
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e5efa2dff18a27131a13cb173e064edad2d0a3c2
--- /dev/null
+++ b/test/gtest_solver_ceres.cpp
@@ -0,0 +1,1522 @@
+/*
+ * gtest_solver_ptr.cpp
+ *
+ *  Created on: Jun, 2018
+ *      Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_angle.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+
+#include "core/solver/solver_manager.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
+
+#include "ceres/ceres.h"
+
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_manager.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+    Vector4d state; state << 1, 0, 0, 0;
+    return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+    return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+                                                                               VectorXd::Zero(sb_ptr->getSize()),
+                                                                               MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+                                                 sb_ptr,
+                                                 nullptr,
+                                                 false);
+}
+
+TEST(SolverCeres, Create)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // check pointers
+    EXPECT_EQ(P, solver_ptr->getProblem());
+
+    // run solver check
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverCeres, FloatingStateBlock_Add)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // notify stateblock again
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFix)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock unfixed
+    EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+
+    // Fix frame
+    sb_ptr->fix();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFixed)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver manager
+    solver_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock localparam
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Remove local param
+    sb_ptr->removeLocalParametrization();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock localparam
+    EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_Remove)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddRemove)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check state block
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check state block
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // update solver
+    solver_ptr->update();
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // update solver
+    solver_ptr->update();
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddUpdated)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Fix
+    sb_ptr->fix();
+
+    // Set State
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Check flags have been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->stateUpdated());
+
+    // == When an ADD is notified: a ADD notification should be stored ==
+
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, ADD);
+
+    // == Insert OTHER notifications ==
+
+    // Set State --> FLAG
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Fix --> FLAG
+    sb_ptr->unfix();
+    // Check flag has been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+    // == consume empties the notification map ==
+    solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    // Check flags have been reset
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+
+    // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
+
+    // == REMOVE + ADD: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+    // == UPDATES should clear the list of notifications ==
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // == ADD + REMOVE: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    P->notifyStateBlock(sb_ptr,REMOVE);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverCeres, StateBlockAndFactor_Add)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock (floating for the moment)
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+    // notify factor (state block now not floating)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // notify stateblock again
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_Fix)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock unfixed
+    EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+
+    // Fix frame
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddFixed)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check solver
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Remove local param
+    sb_ptr->removeLocalParametrization();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check solver
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+    // check notifications
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check state block
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+    // check notifications
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // state block should be automatically stored as floating
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // factor ADD is posponed
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver manager
+    solver_ptr->update(); // repeated REMOVE should be ignored
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check state block
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+    // check notifications
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // state block should be automatically stored as floating
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+
+    // update solver
+    solver_ptr->update(); // repeated REMOVE should be ignored
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // Fix
+    sb_ptr->fix();
+
+    // Change State
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Check flags have been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->stateUpdated());
+
+    // == When an ADD is notified: a ADD notification should be stored ==
+
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // == Insert OTHER notifications ==
+
+    // Set State --> FLAG
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Fix --> FLAG
+    sb_ptr->unfix();
+    // Check flag has been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+    // == consume empties the notification map ==
+    solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    // Check flags have been reset
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+
+    // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
+
+    // == REMOVE + ADD: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+    // == UPDATES should clear the list of notifications ==
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // update solver
+    solver_ptr->update();
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+    // == ADD + REMOVE: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    P->notifyStateBlock(sb_ptr,REMOVE);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverCeres, OnlyFactor_Add)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // checks
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, OnlyFactor_Remove)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // update solver
+    solver_ptr->update(); // nothing to update
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // checks
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, OnlyFactor_AddRemove)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // update solver
+    solver_ptr->update(); // nothing to update
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // checks
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3cd41dde30bc72ecc16fc27d70b8cde5fe327a7b
--- /dev/null
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -0,0 +1,81 @@
+/*
+ * gtest_solver_ptr.cpp
+ *
+ *  Created on: Jun, 2018
+ *      Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_angle.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+
+#include "core/solver/solver_manager.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
+
+#include "ceres/ceres.h"
+
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_manager_multithread.cpp
+ * (modifications should be applied to both tests)
+ */
+
+TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
+{
+    double Dt = 5.0;
+    ProblemPtr P = Problem::create("PO", 2);
+    auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+    // loop updating (without sleep)
+    std::thread t([&](){
+        auto start_t = std::chrono::high_resolution_clock::now();
+        while (true)
+        {
+            solver_ptr->update();
+            ASSERT_TRUE(solver_ptr->check());
+            if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+                break;
+        }});
+
+    // loop emplacing and removing frames (window of 10 KF)
+    auto start = std::chrono::high_resolution_clock::now();
+    TimeStamp ts(0);
+    while (true)
+    {
+        // Emplace Frame, Capture, feature and factor pose 2d
+        FrameBasePtr        F = P->emplaceFrame(ts, P->stateZero());
+        auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+        auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+        auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+        ts += 1.0;
+
+        if (P->getTrajectory()->getFrameMap().size() > 10)
+            (*P->getTrajectory()->begin())->remove();
+
+        if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+            break;
+    }
+
+    t.join();
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 7e212b9971b5bf8fb1a46f94141ce61a1a5aab08..2eb3f46e927270a7e61abb03734d01898722f471 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,8 +13,8 @@
 #include "core/state_block/state_block.h"
 #include "core/capture/capture_void.h"
 #include "core/factor/factor_pose_2d.h"
-#include "core/state_block/local_parametrization_base.h"
-#include "core/state_block/local_parametrization_angle.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
 #include "dummy/solver_manager_dummy.h"
 
 #include <iostream>
@@ -23,568 +23,1490 @@
 using namespace wolf;
 using namespace Eigen;
 
+/*
+ * Following tests are the same as in gtest_solver_ceres.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+    Vector4d state; state << 1, 0, 0, 0;
+    return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+    return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+                                                                               VectorXd::Zero(sb_ptr->getSize()),
+                                                                               MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+                                                 sb_ptr,
+                                                 nullptr,
+                                                 false);
+}
+
 TEST(SolverManager, Create)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
-    // check double pointers to branches
-    ASSERT_EQ(P, solver_manager_ptr->getProblem());
+    // check pointers
+    EXPECT_EQ(P, solver_ptr->getProblem());
+
+    // run solver check
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverManager, FloatingStateBlock_Add)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check stateblock
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, DoubleAddStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleAdd)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
 
-    // add stateblock again
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // notify stateblock again
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check stateblock
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, UpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFix)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock unfixed
-    ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 
     // Fix frame
     sb_ptr->fix();
 
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // update solver manager
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock fixed
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFixed)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // Fix state block
     sb_ptr->fix();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // update solver manager
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock fixed
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // Local param
-    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
     sb_ptr->setLocalParametrization(local_ptr);
 
     // Fix state block
     sb_ptr->fix();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // update solver manager
-    solver_manager_ptr->update();
+    solver_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock fixed
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
-    ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
     // Local param
-    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
     sb_ptr->setLocalParametrization(local_ptr);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver manager
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check stateblock localparam
-    ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // Remove local param
     sb_ptr->removeLocalParametrization();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
 
     // update solver manager
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check stateblock localparam
-    ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, RemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_Remove)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // remove state_block
     P->notifyStateBlock(sb_ptr,REMOVE);
 
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check stateblock
-    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemove)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
+    P->notifyStateBlock(sb_ptr,REMOVE); // should cancel out the ADD
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // check state block
-    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, RemoveUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add state_block
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
+    P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check state block
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, DoubleRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleRemove)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // update solver
+    solver_ptr->update();
+
     // remove state_block
     P->notifyStateBlock(sb_ptr,REMOVE);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
 
     // remove state_block
     P->notifyStateBlock(sb_ptr,REMOVE);
 
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
     // update solver manager
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddUpdatedStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdated)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
     // Fix
     sb_ptr->fix();
 
     // Set State
-    Vector2d state_2 = 2*state;
-    sb_ptr->setState(state_2);
+    sb_ptr->setState(2*sb_ptr->getState());
 
     // Check flags have been set true
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->stateUpdated());
 
     // == When an ADD is notified: a ADD notification should be stored ==
 
-    // add state_block
+    // notify state block
     P->notifyStateBlock(sb_ptr,ADD);
 
+    // check notifications
+    // check notifications
     Notification notif;
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
-    ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
-    ASSERT_EQ(notif, ADD);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, ADD);
 
     // == Insert OTHER notifications ==
 
     // Set State --> FLAG
-    state_2 = 2*state;
-    sb_ptr->setState(state_2);
+    sb_ptr->setState(2*sb_ptr->getState());
 
     // Fix --> FLAG
     sb_ptr->unfix();
     // Check flag has been set true
-    ASSERT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
 
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
 
     // == consume empties the notification map ==
-    solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+    solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
     // Check flags have been reset
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
 
     // == When an REMOVE is notified: a REMOVE notification should be stored ==
 
     // remove state_block
     P->notifyStateBlock(sb_ptr,REMOVE);
 
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
-    ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
-    ASSERT_EQ(notif, REMOVE);
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
 
     // == REMOVE + ADD: notification map should be empty ==
     P->notifyStateBlock(sb_ptr,ADD);
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
 
     // == UPDATES should clear the list of notifications ==
-    // add state_block
+    // notify state block
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
 
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
 
     // == ADD + REMOVE: notification map should be empty ==
     P->notifyStateBlock(sb_ptr,ADD);
     P->notifyStateBlock(sb_ptr,REMOVE);
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverManager, StateBlockAndFactor_Add)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock (floating for the moment)
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+    // notify factor (state block now not floating)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // notify stateblock again
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check stateblock
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_Fix)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock unfixed
+    EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+
+    // Fix frame
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddFixed)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // check solver
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Remove local param
+    sb_ptr->removeLocalParametrization();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_ptr->update();
+
+    // check solver
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+    // check notifications
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+    // update solver
+    solver_ptr->update();
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check state block
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddFactor)
+TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    // notification
+    // check notifications
     Notification notif;
-    ASSERT_TRUE(P->getFactorNotification(c,notif));
-    ASSERT_EQ(notif, ADD);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+    // check notifications
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update(); // state block should be automatically stored as floating
 
-    // check factor
-    ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, RemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update(); // factor ADD is posponed
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver manager
+    solver_ptr->update(); // repeated REMOVE should be ignored
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // check state block
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    // add factor
-    P->notifyFactor(c,REMOVE);
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    // notification
+    // check notifications
     Notification notif;
-    ASSERT_TRUE(P->getFactorNotification(c,notif));
-    ASSERT_EQ(notif, REMOVE);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+    // check notifications
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // update solver
+    solver_ptr->update(); // state block should be automatically stored as floating
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(REMOVE, notif);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update(); // repeated REMOVE should be ignored
 
-    // check factor
-    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+    // checks
+    EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, AddRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // Fix
+    sb_ptr->fix();
+
+    // Change State
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Check flags have been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->stateUpdated());
+
+    // == When an ADD is notified: a ADD notification should be stored ==
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, 0, P->stateZero() );
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    // notification
+    // check notifications
     Notification notif;
-    ASSERT_TRUE(P->getFactorNotification(c,notif));
-    ASSERT_EQ(notif, ADD);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // == Insert OTHER notifications ==
+
+    // Set State --> FLAG
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Fix --> FLAG
+    sb_ptr->unfix();
+    // Check flag has been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+    // == consume empties the notification map ==
+    solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    // Check flags have been reset
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+
+    // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
 
-    // add factor
-    P->notifyFactor(c,REMOVE);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
 
-    // notification
-    ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
-    ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
+    // == REMOVE + ADD: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+    // == UPDATES should clear the list of notifications ==
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update();
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
 
-    // check factor
-    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+    // == ADD + REMOVE: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    P->notifyStateBlock(sb_ptr,REMOVE);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
 }
 
-TEST(SolverManager, DoubleRemoveFactor)
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverManager, OnlyFactor_Add)
 {
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero() );
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
 
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
 
-    // remove factor
-    P->notifyFactor(c,REMOVE);
+    // checks
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_Remove)
+{
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
 
-    // remove factor
-    P->notifyFactor(c,REMOVE);
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
 
     // update solver
-    solver_manager_ptr->update();
+    solver_ptr->update(); // nothing to update
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
 
-    // check factor
-    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+    // checks
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
-TEST(SolverManager, MultiThreadingTruncatedNotifications)
+TEST(SolverManager, OnlyFactor_AddRemove)
 {
-    double Dt = 5.0;
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
-
-    // loop updating (without sleep)
-    std::thread t([&](){
-        auto start_t = std::chrono::high_resolution_clock::now();
-        while (true)
-        {
-            solver_manager_ptr->update();
-            if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
-                break;
-        }});
-
-    // loop emplacing and removing frames (window of 10 KF)
-    auto start = std::chrono::high_resolution_clock::now();
-    while (true)
-    {
-        // Emplace Frame, Capture, feature and factor pose 2d
-        FrameBasePtr        F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero());
-        auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-        auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
-        auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
-
-        if (P->getTrajectory()->getFrameList().size() > 10)
-            P->getTrajectory()->getFrameList().front()->remove();
-
-        if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
-            break;
-    }
-
-    t.join();
+    ProblemPtr P = Problem::create("PO", 3);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check notifications
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(ADD, notif);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // update solver
+    solver_ptr->update(); // nothing to update
+
+    // check notifications
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+    // checks
+    EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_ptr->check());
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4e772c1bb74747d80a6ceda04d62a679f323a4e3
--- /dev/null
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -0,0 +1,75 @@
+/*
+ * gtest_solver_manager.cpp
+ *
+ *  Created on: Jun, 2018
+ *      Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+#include "dummy/solver_manager_dummy.h"
+
+#include <iostream>
+#include <thread>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_ceres_multithread.cpp
+ * (modifications should be applied to both tests)
+ */
+
+TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
+{
+    double Dt = 5.0;
+    ProblemPtr P = Problem::create("PO", 2);
+    auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // loop updating (without sleep)
+    std::thread t([&](){
+        auto start_t = std::chrono::high_resolution_clock::now();
+        while (true)
+        {
+            solver_ptr->update();
+            ASSERT_TRUE(solver_ptr->check());
+            if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+                break;
+        }});
+
+    // loop emplacing and removing frames (window of 10 KF)
+    auto start = std::chrono::high_resolution_clock::now();
+    TimeStamp ts(0);
+    while (true)
+    {
+        // Emplace Frame, Capture, feature and factor pose 2d
+        FrameBasePtr        F = P->emplaceFrame(ts, P->stateZero());
+        auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+        auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+        auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+        ts += 1.0;
+
+        if (P->getTrajectory()->getFrameMap().size() > 10)
+            (*P->getTrajectory()->begin())->remove();
+
+        if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+            break;
+    }
+
+    t.join();
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 56f705aa3b9c3689a076989fcd8e30896d80fe13..f4bcea67bd4e51af793e026f1496f2591fcfe98a 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -25,9 +25,9 @@ class StateBlockCompositeInit : public testing::Test
 
         void SetUp() override
         {
-            sbp = states.emplace("P", Vector3d(1,2,3));
-            sbv = states.emplace("V", Vector3d(4,5,6));
-            sbq = states.emplace<StateQuaternion>("Q", Vector4d(.5,.5,.5,.5));
+            sbp = states.emplace('P', Vector3d(1,2,3));
+            sbv = states.emplace('V', Vector3d(4,5,6));
+            sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
 
             sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
         }
@@ -57,8 +57,8 @@ TEST_F(StateBlockCompositeInit, emplace)
 
 TEST_F(StateBlockCompositeInit, has_key)
 {
-    ASSERT_TRUE(states.has("P"));
-    ASSERT_FALSE(states.has("X"));
+    ASSERT_TRUE(states.has('P'));
+    ASSERT_FALSE(states.has('X'));
 }
 
 TEST_F(StateBlockCompositeInit, has_sb)
@@ -69,22 +69,22 @@ TEST_F(StateBlockCompositeInit, has_sb)
 
 TEST_F(StateBlockCompositeInit, at)
 {
-    ASSERT_EQ(states.at("P"), sbp);
+    ASSERT_EQ(states.at('P'), sbp);
 
-    ASSERT_EQ(states.at("X"), nullptr);
+    ASSERT_EQ(states.at('X'), nullptr);
 }
 
 TEST_F(StateBlockCompositeInit, set_sb)
 {
-    states.set("P", sbx);
+    states.set('P', sbx);
 
-    ASSERT_EQ(states.at("P"), sbx);
+    ASSERT_EQ(states.at('P'), sbx);
 
-    states.set("P", sbp);
+    states.set('P', sbp);
 
-    ASSERT_DEATH(states.set("X", sbx), "");
+    ASSERT_DEATH(states.set('X', sbx), "");
 
-    ASSERT_EQ(states.at("X"), nullptr);
+    ASSERT_EQ(states.at('X'), nullptr);
 }
 
 TEST_F(StateBlockCompositeInit, set_vec)
@@ -92,13 +92,13 @@ TEST_F(StateBlockCompositeInit, set_vec)
     Vector3d p(11,22,33);
     Vector3d x(55,66,77);
 
-    states.set("P", p);
+    states.set('P', p);
 
-    ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
+    ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
 
-    ASSERT_DEATH(states.set("X", x), "");
+    ASSERT_DEATH(states.set('X', x), "");
 
-    ASSERT_EQ(states.at("X"), nullptr);
+    ASSERT_EQ(states.at('X'), nullptr);
 }
 
 TEST_F(StateBlockCompositeInit, set_vectors)
@@ -109,31 +109,31 @@ TEST_F(StateBlockCompositeInit, set_vectors)
 
     states.setVectors("PQ", {p, q});
 
-    ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
-    ASSERT_MATRIX_APPROX(states.at("Q")->getState(), q, 1e-20);
+    ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
+    ASSERT_MATRIX_APPROX(states.at('Q')->getState(), q, 1e-20);
 
     ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
 
-    ASSERT_EQ(states.at("X"), nullptr);
+    ASSERT_EQ(states.at('X'), nullptr);
 }
 
 TEST_F(StateBlockCompositeInit, key_ref)
 {
-    std::string key;
+    char key;
     ASSERT_TRUE(states.key(sbp, key));
-    ASSERT_EQ(key, "P");
+    ASSERT_EQ(key, 'P');
 
     ASSERT_FALSE(states.key(sbx, key));
-    ASSERT_EQ(key, "");
+    ASSERT_EQ(key, '0');
 }
 
 TEST_F(StateBlockCompositeInit, key_return)
 {
     // existing key
-    ASSERT_EQ(states.key(sbp), "P");
+    ASSERT_EQ(states.key(sbp), 'P');
 
-    // non existing key returns empty string
-    ASSERT_EQ(states.key(sbx), "");
+    // non existing key returns zero char
+    ASSERT_EQ(states.key(sbx), '0');
 }
 
 TEST_F(StateBlockCompositeInit, find)
@@ -147,81 +147,81 @@ TEST_F(StateBlockCompositeInit, find)
 
 TEST_F(StateBlockCompositeInit, add)
 {
-    states.add("X", sbx);
+    states.add('X', sbx);
 
-    ASSERT_EQ(states.at("X"), sbx);
+    ASSERT_EQ(states.at('X'), sbx);
 }
 
 TEST_F(StateBlockCompositeInit, remove)
 {
     // remove existing block
-    states.remove("V");
+    states.remove('V');
     ASSERT_EQ(states.getStateBlockMap().size(), 2);
 
     // remove non existing block -- no effect
-    states.remove("X");
+    states.remove('X');
     ASSERT_EQ(states.getStateBlockMap().size(), 2);
 }
 
 TEST_F(StateBlockCompositeInit, perturb)
 {
-    ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
-    ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
-    ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+    ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
+    ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
+    ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
 
     states.perturb(0.1);
 
     // values have moved wrt original
-    ASSERT_FALSE(states.at("P")->getState().isApprox(Vector3d(1,2,3), 1e-3));
-    ASSERT_FALSE(states.at("V")->getState().isApprox(Vector3d(4,5,6), 1e-3));
-    ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
+    ASSERT_FALSE(states.at('P')->getState().isApprox(Vector3d(1,2,3), 1e-3));
+    ASSERT_FALSE(states.at('V')->getState().isApprox(Vector3d(4,5,6), 1e-3));
+    ASSERT_FALSE(states.at('Q')->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
 }
 
 TEST_F(StateBlockCompositeInit, setIdentity)
 {
-    ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
-    ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
-    ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+    ASSERT_TRUE(states.at('P')->getState().isApprox(sbp->getState(), 1e-3));
+    ASSERT_TRUE(states.at('V')->getState().isApprox(sbv->getState(), 1e-3));
+    ASSERT_TRUE(states.at('Q')->getState().isApprox(sbq->getState(), 1e-3));
 
     states.setIdentity();
 
     // values have moved wrt original
-    ASSERT_TRUE(states.at("P")->getState().isApprox(Vector3d(0,0,0), 1e-10));
-    ASSERT_TRUE(states.at("V")->getState().isApprox(Vector3d(0,0,0), 1e-10));
-    ASSERT_TRUE(states.at("Q")->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
+    ASSERT_TRUE(states.at('P')->getState().isApprox(Vector3d(0,0,0), 1e-10));
+    ASSERT_TRUE(states.at('V')->getState().isApprox(Vector3d(0,0,0), 1e-10));
+    ASSERT_TRUE(states.at('Q')->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
 }
 
 TEST_F(StateBlockCompositeInit, identity)
 {
     VectorComposite v = states.identity();
 
-    ASSERT_TRUE(v.at("P").isApprox(Vector3d(0,0,0), 1e-10));
-    ASSERT_TRUE(v.at("V").isApprox(Vector3d(0,0,0), 1e-10));
-    ASSERT_TRUE(v.at("Q").isApprox(Vector4d(0,0,0,1), 1e-10));
+    ASSERT_TRUE(v.at('P').isApprox(Vector3d(0,0,0), 1e-10));
+    ASSERT_TRUE(v.at('V').isApprox(Vector3d(0,0,0), 1e-10));
+    ASSERT_TRUE(v.at('Q').isApprox(Vector4d(0,0,0,1), 1e-10));
 }
 
 TEST_F(StateBlockCompositeInit, fix)
 {
     states.fix();
 
-    ASSERT_TRUE(states.at("P")->isFixed());
-    ASSERT_TRUE(states.at("V")->isFixed());
-    ASSERT_TRUE(states.at("Q")->isFixed());
+    ASSERT_TRUE(states.at('P')->isFixed());
+    ASSERT_TRUE(states.at('V')->isFixed());
+    ASSERT_TRUE(states.at('Q')->isFixed());
 }
 
 TEST_F(StateBlockCompositeInit, unfix)
 {
     states.fix();
 
-    ASSERT_TRUE(states.at("P")->isFixed());
-    ASSERT_TRUE(states.at("V")->isFixed());
-    ASSERT_TRUE(states.at("Q")->isFixed());
+    ASSERT_TRUE(states.at('P')->isFixed());
+    ASSERT_TRUE(states.at('V')->isFixed());
+    ASSERT_TRUE(states.at('Q')->isFixed());
 
     states.unfix();
 
-    ASSERT_FALSE(states.at("P")->isFixed());
-    ASSERT_FALSE(states.at("V")->isFixed());
-    ASSERT_FALSE(states.at("Q")->isFixed());
+    ASSERT_FALSE(states.at('P')->isFixed());
+    ASSERT_FALSE(states.at('V')->isFixed());
+    ASSERT_FALSE(states.at('Q')->isFixed());
 }
 
 TEST_F(StateBlockCompositeInit, isFixed)
@@ -230,7 +230,7 @@ TEST_F(StateBlockCompositeInit, isFixed)
 
     ASSERT_TRUE(states.isFixed());
 
-    states.at("P")->unfix();
+    states.at('P')->unfix();
 
     ASSERT_FALSE(states.isFixed());
 }
@@ -245,23 +245,23 @@ TEST(VectorComposite, constructor_empty)
 TEST(VectorComposite, constructor_copy)
 {
     VectorComposite u;
-    u.emplace("a", Vector2d(1,2));
-    u.emplace("b", Vector3d(3,4,5));
+    u.emplace('a', Vector2d(1,2));
+    u.emplace('b', Vector3d(3,4,5));
 
     VectorComposite v(u);
 
     ASSERT_FALSE(v.empty());
 
-    ASSERT_MATRIX_APPROX(u["a"], v["a"], 1e-20);
-    ASSERT_MATRIX_APPROX(u["b"], v["b"], 1e-20);
+    ASSERT_MATRIX_APPROX(u['a'], v['a'], 1e-20);
+    ASSERT_MATRIX_APPROX(u['b'], v['b'], 1e-20);
 }
 
 TEST(VectorComposite, constructor_from_list)
 {
     VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1});
 
-    ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
-    ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+    ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20);
+    ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
 }
 
 TEST(VectorComposite, set)
@@ -270,8 +270,8 @@ TEST(VectorComposite, set)
 
     v.set(Vector4d(1,2,3,4), "ab", {3,1});
 
-    ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
-    ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+    ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1,2,3), 1e-20);
+    ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
 }
 
 TEST(VectorComposite, operatorStream)
@@ -280,8 +280,8 @@ TEST(VectorComposite, operatorStream)
 
     VectorComposite x;
 
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
+    x.emplace('P', Vector2d(1,1));
+    x.emplace('O', Vector3d(2,2,2));
 
     WOLF_DEBUG("X = ", x);
 }
@@ -290,54 +290,54 @@ TEST(VectorComposite, operatorPlus)
 {
     VectorComposite x, y;
 
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
-    y.emplace("P", Vector2d(1,1));
-    y.emplace("O", Vector3d(2,2,2));
+    x.emplace('P', Vector2d(1,1));
+    x.emplace('O', Vector3d(2,2,2));
+    y.emplace('P', Vector2d(1,1));
+    y.emplace('O', Vector3d(2,2,2));
 
     WOLF_DEBUG("x + y = ", x + y);
 
-    ASSERT_MATRIX_APPROX((x+y).at("P"), Vector2d(2,2), 1e-20);
-    ASSERT_MATRIX_APPROX((x+y).at("O"), Vector3d(4,4,4), 1e-20);
+    ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20);
+    ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20);
 }
 
 TEST(VectorComposite, operatorMinus)
 {
     VectorComposite x, y;
 
-    x.emplace("P", Vector2d(3,3));
-    x.emplace("O", Vector3d(6,6,6));
-    y.emplace("P", Vector2d(1,1));
-    y.emplace("O", Vector3d(2,2,2));
+    x.emplace('P', Vector2d(3,3));
+    x.emplace('O', Vector3d(6,6,6));
+    y.emplace('P', Vector2d(1,1));
+    y.emplace('O', Vector3d(2,2,2));
 
     WOLF_DEBUG("x = ", x);
     WOLF_DEBUG("y = ", y);
     WOLF_DEBUG("x - y = ", x - y);
 
-    ASSERT_MATRIX_APPROX((x-y).at("P"), Vector2d(2,2), 1e-20);
-    ASSERT_MATRIX_APPROX((x-y).at("O"), Vector3d(4,4,4), 1e-20);
+    ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20);
+    ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20);
 }
 
 TEST(VectorComposite, unary_Minus)
 {
     VectorComposite x, y;
 
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
+    x.emplace('P', Vector2d(1,1));
+    x.emplace('O', Vector3d(2,2,2));
 
     WOLF_DEBUG("-x = ", -x);
 
-    ASSERT_MATRIX_APPROX((-x).at("P"), Vector2d(-1,-1), 1e-20);
-    ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
+    ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20);
+    ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20);
 }
 
 TEST(VectorComposite, stateVector)
 {
     VectorComposite x;
 
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
-    x.emplace("V", Vector4d(3,3,3,3));
+    x.emplace('P', Vector2d(1,1));
+    x.emplace('O', Vector3d(2,2,2));
+    x.emplace('V', Vector4d(3,3,3,3));
 
     VectorXd y(5); y<<1,1,2,2,2;
     ASSERT_MATRIX_APPROX(x.vector("PO"), y, 1e-20);
@@ -365,7 +365,7 @@ TEST(MatrixComposite, Constructor_structure)
     MatrixComposite M("PO", "POV");
 
     ASSERT_EQ(M.size() , 2);
-    ASSERT_EQ(M.at("P").size() , 3);
+    ASSERT_EQ(M.at('P').size() , 3);
 
     ASSERT_TRUE(M.check());
 }
@@ -375,10 +375,10 @@ TEST(MatrixComposite, Constructor_structure_sizes)
     MatrixComposite M("PO", {3,4}, "POV", {3,4,3});
 
     ASSERT_EQ(M.size() , 2);
-    ASSERT_EQ(M.at("P").size() , 3);
+    ASSERT_EQ(M.at('P').size() , 3);
 
-    ASSERT_EQ(M.at("P","O").rows() , 3);
-    ASSERT_EQ(M.at("P","O").cols() , 4);
+    ASSERT_EQ(M.at('P','O').rows() , 3);
+    ASSERT_EQ(M.at('P','O').cols() , 4);
 
     ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
     ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
@@ -393,16 +393,16 @@ TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes)
     MatrixComposite M(m, "PO", {3,4}, "POV", {3,4,3});
 
     ASSERT_EQ(M.size() , 2);
-    ASSERT_EQ(M.at("P").size() , 3);
+    ASSERT_EQ(M.at('P').size() , 3);
 
-    ASSERT_EQ(M.at("P","O").rows() , 3);
-    ASSERT_EQ(M.at("P","O").cols() , 4);
+    ASSERT_EQ(M.at('P','O').rows() , 3);
+    ASSERT_EQ(M.at('P','O').cols() , 4);
 
     ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
     ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
 
-    ASSERT_MATRIX_APPROX(M.at("P","O"), m.block(0,3,3,4), 1e-20);
-    ASSERT_MATRIX_APPROX(M.at("O","V"), m.block(3,7,4,3), 1e-20);
+    ASSERT_MATRIX_APPROX(M.at('P','O'), m.block(0,3,3,4), 1e-20);
+    ASSERT_MATRIX_APPROX(M.at('O','V'), m.block(3,7,4,3), 1e-20);
 
     ASSERT_TRUE(M.check());
 }
@@ -462,10 +462,10 @@ TEST(MatrixComposite, emplace_operatorStream)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    ASSERT_TRUE(M.emplace("P", "P", Mpp));
-    ASSERT_TRUE(M.emplace("P", "O", Mpo));
-    ASSERT_TRUE(M.emplace("O", "P", Mop));
-    ASSERT_TRUE(M.emplace("O", "O", Moo));
+    ASSERT_TRUE(M.emplace('P', 'P', Mpp));
+    ASSERT_TRUE(M.emplace('P', 'O', Mpo));
+    ASSERT_TRUE(M.emplace('O', 'P', Mop));
+    ASSERT_TRUE(M.emplace('O', 'O', Moo));
 
     cout << "M = " << M << endl;
 }
@@ -485,14 +485,14 @@ TEST(MatrixComposite, emplace_operatorStream)
 //    Mop.setOnes(); Mop *= 3;
 //    Moo.setOnes(); Moo *= 4;
 //
-//    M.emplace("P", "P", Mpp);
-//    ASSERT_MATRIX_APPROX( M["P"]["P"], Mpp, 1e-20);
+//    M.emplace('P', 'P', Mpp);
+//    ASSERT_MATRIX_APPROX( M['P']['P'], Mpp, 1e-20);
 //
-//    M.emplace("P", "O", Mpo);
-//    ASSERT_MATRIX_APPROX( M["P"]["O"], Mpo, 1e-20);
+//    M.emplace('P', 'O', Mpo);
+//    ASSERT_MATRIX_APPROX( M['P']['O'], Mpo, 1e-20);
 //
 //    // return default empty matrix if block not present
-//    MatrixXd N = M["O"]["O"];
+//    MatrixXd N = M['O']['O'];
 //    ASSERT_EQ(N.rows(), 0);
 //    ASSERT_EQ(N.cols(), 0);
 //}
@@ -512,14 +512,14 @@ TEST(MatrixComposite, emplace_operatorStream)
 //    Mop.setOnes(); Mop *= 3;
 //    Moo.setOnes(); Moo *= 4;
 //
-//    M.emplace("P", "P", Mpp);
-//    ASSERT_MATRIX_APPROX( M("P", "P"), Mpp, 1e-20);
+//    M.emplace('P', 'P', Mpp);
+//    ASSERT_MATRIX_APPROX( M('P', 'P'), Mpp, 1e-20);
 //
-//    M.emplace("P", "O", Mpo);
-//    ASSERT_MATRIX_APPROX( M("P", "O"), Mpo, 1e-20);
+//    M.emplace('P', 'O', Mpo);
+//    ASSERT_MATRIX_APPROX( M('P', 'O'), Mpo, 1e-20);
 //
 //    // return default empty matrix if block not present
-//    MatrixXd N = M("O", "O");
+//    MatrixXd N = M('O', 'O');
 //    ASSERT_EQ(N.rows(), 0);
 //    ASSERT_EQ(N.cols(), 0);
 //}
@@ -539,14 +539,14 @@ TEST(MatrixComposite, operatorAt)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    ASSERT_MATRIX_APPROX( M.at("P", "P"), Mpp, 1e-20);
+    M.emplace('P', 'P', Mpp);
+    ASSERT_MATRIX_APPROX( M.at('P', 'P'), Mpp, 1e-20);
 
-    M.emplace("P", "O", Mpo);
-    ASSERT_MATRIX_APPROX( M.at("P", "O"), Mpo, 1e-20);
+    M.emplace('P', 'O', Mpo);
+    ASSERT_MATRIX_APPROX( M.at('P', 'O'), Mpo, 1e-20);
 
     // error if block not present
-    ASSERT_DEATH(MatrixXd N = M.at("O", "O"), "");
+    ASSERT_DEATH(MatrixXd N = M.at('O', 'O'), "");
 }
 
 TEST(MatrixComposite, productScalar)
@@ -563,10 +563,10 @@ TEST(MatrixComposite, productScalar)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    M.emplace("P", "O", Mpo);
-    M.emplace("O", "P", Mop);
-    M.emplace("O", "O", Moo);
+    M.emplace('P', 'P', Mpp);
+    M.emplace('P', 'O', Mpo);
+    M.emplace('O', 'P', Mop);
+    M.emplace('O', 'O', Moo);
     WOLF_DEBUG("M = " , M);
 
 
@@ -589,8 +589,8 @@ TEST(MatrixComposite, productVector)
     osize = 3;
 
     VectorComposite x;
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
+    x.emplace('P', Vector2d(1,1));
+    x.emplace('O', Vector3d(2,2,2));
 
     cout << "x= " << x << endl;
 
@@ -604,10 +604,10 @@ TEST(MatrixComposite, productVector)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    M.emplace("P", "O", Mpo);
-    M.emplace("O", "P", Mop);
-    M.emplace("O", "O", Moo);
+    M.emplace('P', 'P', Mpp);
+    M.emplace('P', 'O', Mpo);
+    M.emplace('O', 'P', Mop);
+    M.emplace('O', 'O', Moo);
     WOLF_DEBUG("M = " , M);
 
     VectorComposite y;
@@ -628,15 +628,15 @@ TEST(MatrixComposite, productVector)
     Vector2d yp(14,14);
     Vector3d yo(30,30,30);
 
-    ASSERT_MATRIX_APPROX(y.at("P"), yp, 1e-20);
-    ASSERT_MATRIX_APPROX(y.at("O"), yo, 1e-20);
+    ASSERT_MATRIX_APPROX(y.at('P'), yp, 1e-20);
+    ASSERT_MATRIX_APPROX(y.at('O'), yo, 1e-20);
 
     // throw if x has extra blocks
-    // x.emplace("V", Vector2d(3,3));
+    // x.emplace('V', Vector2d(3,3));
     // ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M
 
     // throw if x has missing blocks
-    // x.erase("O");
+    // x.erase('O');
     // cout << "x = " << x << endl;
     // ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH
 
@@ -657,10 +657,10 @@ TEST(MatrixComposite, product)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    M.emplace("P", "O", Mpo);
-    M.emplace("O", "P", Mop);
-    M.emplace("O", "O", Moo);
+    M.emplace('P', 'P', Mpp);
+    M.emplace('P', 'O', Mpo);
+    M.emplace('O', 'P', Mop);
+    M.emplace('O', 'O', Moo);
     WOLF_DEBUG("M = " , M);
 
     MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize);
@@ -669,10 +669,10 @@ TEST(MatrixComposite, product)
     Npo.setOnes(); Npo *= 3;
     Npv.setOnes(); Npv *= 4;
 
-    N.emplace("O", "O", Noo);
-    N.emplace("O", "V", Nov);
-    N.emplace("P", "O", Npo);
-    N.emplace("P", "V", Npv);
+    N.emplace('O', 'O', Noo);
+    N.emplace('O', 'V', Nov);
+    N.emplace('P', 'O', Npo);
+    N.emplace('P', 'V', Npv);
     WOLF_DEBUG("N = " , N);
 
     MatrixComposite MN;
@@ -693,10 +693,10 @@ TEST(MatrixComposite, product)
     MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);
     MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32);
 
-    ASSERT_MATRIX_APPROX(MN.at("P","O"), MNpo, 1e-20);
-    ASSERT_MATRIX_APPROX(MN.at("P","V"), MNpv, 1e-20);
-    ASSERT_MATRIX_APPROX(MN.at("O","O"), MNoo, 1e-20);
-    ASSERT_MATRIX_APPROX(MN.at("O","V"), MNov, 1e-20);
+    ASSERT_MATRIX_APPROX(MN.at('P','O'), MNpo, 1e-20);
+    ASSERT_MATRIX_APPROX(MN.at('P','V'), MNpv, 1e-20);
+    ASSERT_MATRIX_APPROX(MN.at('O','O'), MNoo, 1e-20);
+    ASSERT_MATRIX_APPROX(MN.at('O','V'), MNov, 1e-20);
 
     ASSERT_TRUE(MN.check());
 }
@@ -717,10 +717,10 @@ TEST(MatrixComposite, propagate)
     Qop.setOnes(); Qop *= 2;
     Qoo.setOnes(); Qoo *= 3;
 
-    Q.emplace("P", "P", Qpp);
-    Q.emplace("P", "O", Qpo);
-    Q.emplace("O", "P", Qop);
-    Q.emplace("O", "O", Qoo);
+    Q.emplace('P', 'P', Qpp);
+    Q.emplace('P', 'O', Qpo);
+    Q.emplace('O', 'P', Qop);
+    Q.emplace('O', 'O', Qoo);
     WOLF_DEBUG("Q = " , Q);
 
     MatrixXd Jvp(vsize,psize), Jvo(vsize, osize), Jwp(wsize,psize), Jwo(wsize,osize);
@@ -729,10 +729,10 @@ TEST(MatrixComposite, propagate)
     Jwp.setOnes(); Jwp *= 3;
     Jwo.setOnes(); Jwo *= 4;
 
-    J.emplace("V", "P", Jvp);
-    J.emplace("V", "O", Jvo);
-    J.emplace("W", "P", Jwp);
-    J.emplace("W", "O", Jwo);
+    J.emplace('V', 'P', Jvp);
+    J.emplace('V', 'O', Jvo);
+    J.emplace('W', 'P', Jwp);
+    J.emplace('W', 'O', Jwo);
     WOLF_DEBUG("J = " , J);
 
     MatrixComposite JQJt;
@@ -763,15 +763,15 @@ TEST(MatrixComposite, sum)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    M.emplace("P", "O", Mpo);
-    M.emplace("O", "P", Mop);
-    M.emplace("O", "O", Moo);
+    M.emplace('P', 'P', Mpp);
+    M.emplace('P', 'O', Mpo);
+    M.emplace('O', 'P', Mop);
+    M.emplace('O', 'O', Moo);
     WOLF_DEBUG("M = " , M);
-    N.emplace("P", "P", Mpp);
-    N.emplace("P", "O", Mpo);
-    N.emplace("O", "P", Mop);
-    N.emplace("O", "O", Moo);
+    N.emplace('P', 'P', Mpp);
+    N.emplace('P', 'O', Mpo);
+    N.emplace('O', 'P', Mop);
+    N.emplace('O', 'O', Moo);
     WOLF_DEBUG("N = " , N);
 
     MatrixComposite MpN;
@@ -780,10 +780,10 @@ TEST(MatrixComposite, sum)
 
     WOLF_DEBUG("MpN = M + N = " , MpN);
 
-    ASSERT_MATRIX_APPROX(MpN.at("P","P"), 2 * Mpp, 1e-10);
-    ASSERT_MATRIX_APPROX(MpN.at("P","O"), 2 * Mpo, 1e-10);
-    ASSERT_MATRIX_APPROX(MpN.at("O","P"), 2 * Mop, 1e-10);
-    ASSERT_MATRIX_APPROX(MpN.at("O","O"), 2 * Moo, 1e-10);
+    ASSERT_MATRIX_APPROX(MpN.at('P','P'), 2 * Mpp, 1e-10);
+    ASSERT_MATRIX_APPROX(MpN.at('P','O'), 2 * Mpo, 1e-10);
+    ASSERT_MATRIX_APPROX(MpN.at('O','P'), 2 * Mop, 1e-10);
+    ASSERT_MATRIX_APPROX(MpN.at('O','O'), 2 * Moo, 1e-10);
 
 }
 
@@ -801,15 +801,15 @@ TEST(MatrixComposite, difference)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    M.emplace("P", "O", Mpo);
-    M.emplace("O", "P", Mop);
-    M.emplace("O", "O", Moo);
+    M.emplace('P', 'P', Mpp);
+    M.emplace('P', 'O', Mpo);
+    M.emplace('O', 'P', Mop);
+    M.emplace('O', 'O', Moo);
     WOLF_DEBUG("M = " , M);
-    N.emplace("P", "P", Mpp);
-    N.emplace("P", "O", Mpo);
-    N.emplace("O", "P", Mop);
-    N.emplace("O", "O", Moo);
+    N.emplace('P', 'P', Mpp);
+    N.emplace('P', 'O', Mpo);
+    N.emplace('O', 'P', Mop);
+    N.emplace('O', 'O', Moo);
     WOLF_DEBUG("N = " , N);
 
     MatrixComposite MmN;
@@ -818,10 +818,10 @@ TEST(MatrixComposite, difference)
 
     WOLF_DEBUG("MmN = M - N = " , MmN);
 
-    ASSERT_MATRIX_APPROX(MmN.at("P","P"), Mpp * 0, 1e-10);
-    ASSERT_MATRIX_APPROX(MmN.at("P","O"), Mpo * 0, 1e-10);
-    ASSERT_MATRIX_APPROX(MmN.at("O","P"), Mop * 0, 1e-10);
-    ASSERT_MATRIX_APPROX(MmN.at("O","O"), Moo * 0, 1e-10);
+    ASSERT_MATRIX_APPROX(MmN.at('P','P'), Mpp * 0, 1e-10);
+    ASSERT_MATRIX_APPROX(MmN.at('P','O'), Mpo * 0, 1e-10);
+    ASSERT_MATRIX_APPROX(MmN.at('O','P'), Mop * 0, 1e-10);
+    ASSERT_MATRIX_APPROX(MmN.at('O','O'), Moo * 0, 1e-10);
 
 }
 
@@ -839,10 +839,10 @@ TEST(MatrixComposite, unary_minus)
     Mop.setOnes(); Mop *= 3;
     Moo.setOnes(); Moo *= 4;
 
-    M.emplace("P", "P", Mpp);
-    M.emplace("P", "O", Mpo);
-    M.emplace("O", "P", Mop);
-    M.emplace("O", "O", Moo);
+    M.emplace('P', 'P', Mpp);
+    M.emplace('P', 'O', Mpo);
+    M.emplace('O', 'P', Mop);
+    M.emplace('O', 'O', Moo);
     WOLF_DEBUG("M = " , M);
 
     MatrixComposite m;
@@ -851,10 +851,10 @@ TEST(MatrixComposite, unary_minus)
 
     WOLF_DEBUG("m = - M = " , m);
 
-    ASSERT_MATRIX_APPROX(m.at("P","P"), - M.at("P","P"), 1e-10);
-    ASSERT_MATRIX_APPROX(m.at("P","O"), - M.at("P","O"), 1e-10);
-    ASSERT_MATRIX_APPROX(m.at("O","P"), - M.at("O","P"), 1e-10);
-    ASSERT_MATRIX_APPROX(m.at("O","O"), - M.at("O","O"), 1e-10);
+    ASSERT_MATRIX_APPROX(m.at('P','P'), - M.at('P','P'), 1e-10);
+    ASSERT_MATRIX_APPROX(m.at('P','O'), - M.at('P','O'), 1e-10);
+    ASSERT_MATRIX_APPROX(m.at('O','P'), - M.at('O','P'), 1e-10);
+    ASSERT_MATRIX_APPROX(m.at('O','O'), - M.at('O','O'), 1e-10);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 4e539e24b7554a7a2e5cfe88096cc4c861de4018..a14c436ddc0864f7b55e1ef9db1fd451ecf494ea 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -22,9 +22,11 @@ class TrackMatrixTest : public testing::Test
         FrameBasePtr   F0, F1, F2, F3, F4;
         CaptureBasePtr C0, C1, C2, C3, C4;
         FeatureBasePtr f0, f1, f2, f3, f4;
+        ProblemPtr problem;
 
         void SetUp() override
         {
+            problem = Problem::create("PO", 2);
             // unlinked captures
             // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
             C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
@@ -35,11 +37,11 @@ class TrackMatrixTest : public testing::Test
 
             // unlinked frames
             // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
-            F0 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 0.0, nullptr);
-            F1 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 1.0, nullptr);
-            F2 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 2.0, nullptr);
-            F3 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 3.0, nullptr);
-            F4 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 4.0, nullptr);
+            F0 = std::make_shared<FrameBase>(0.0, nullptr);
+            F1 = std::make_shared<FrameBase>(1.0, nullptr);
+            F2 = std::make_shared<FrameBase>(2.0, nullptr);
+            F3 = std::make_shared<FrameBase>(3.0, nullptr);
+            F4 = std::make_shared<FrameBase>(4.0, nullptr);
 
             // unlinked features
             // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
@@ -50,8 +52,8 @@ class TrackMatrixTest : public testing::Test
             f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
 
             // F0 and F4 are keyframes
-            F0->setKey();
-            F4->setKey();
+            F0->link(problem);
+            F4->link(problem);
 
             // link captures
             C0->link(F0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 00f7d83fff99ff39b16fc8a67c2eee9b767cfbe1..bd05620f6fb13bdaa20a97343c12141f44d60c97 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -32,62 +32,34 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     //   1     2     3     4       time stamps
     // --+-----+-----+-----+--->   time
 
-    FrameBasePtr F1 = P->emplaceFrame(KEY,           1, Eigen::Vector3d::Zero() );
-    FrameBasePtr F2 = P->emplaceFrame(KEY,           2, Eigen::Vector3d::Zero() );
-    FrameBasePtr F3 = P->emplaceFrame(AUXILIARY,     3, Eigen::Vector3d::Zero() );
-    FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, 4, Eigen::Vector3d::Zero() );
+    FrameBasePtr F1 = P->emplaceFrame(          1, Eigen::Vector3d::Zero() );
+    FrameBasePtr F2 = P->emplaceFrame(          2, Eigen::Vector3d::Zero() );
+    // FrameBasePtr F3 = P->emplaceFrame(AUXILIARY,     3, Eigen::Vector3d::Zero() );
+    FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+                                                  //                                                              P->getDim(),
+                                                  std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
+    FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameStructure(),
+                                                  //                                                              P->getDim(),
+                                                  std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
 
     FrameBasePtr KF; // closest key-frame queried
 
-    KF = T->closestKeyFrameToTimeStamp(-0.8);                // before all keyframes    --> return F1
+    KF = T->closestFrameToTimeStamp(-0.8);                // before all keyframes    --> return F1
     ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    KF = T->closestKeyFrameToTimeStamp(1.1);                 // between keyframes       --> return F1
+    KF = T->closestFrameToTimeStamp(1.1);                 // between keyframes       --> return F1
     ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    KF = T->closestKeyFrameToTimeStamp(1.9);                 // between keyframes       --> return F2
+    KF = T->closestFrameToTimeStamp(1.9);                 // between keyframes       --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    KF = T->closestKeyFrameToTimeStamp(2.6);                 // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+    KF = T->closestFrameToTimeStamp(2.6);                 // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    KF = T->closestKeyFrameToTimeStamp(3.2);                 // after the auxiliary frame, between closer to frame --> return F2
+    KF = T->closestFrameToTimeStamp(3.2);                 // after the auxiliary frame, between closer to frame --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    KF = T->closestKeyFrameToTimeStamp(4.2);                 // after the last frame --> return F2
-    ASSERT_EQ(KF->id(), F2->id());                           // same id!
-}
-
-TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
-{
-
-    ProblemPtr P = Problem::create("PO", 2);
-    TrajectoryBasePtr T = P->getTrajectory();
-
-    // Trajectory status:
-    //  KF1   KF2    F3      frames
-    //   1     2     3       time stamps
-    // --+-----+-----+--->   time
-
-    FrameBasePtr F1 = P->emplaceFrame(KEY,           1, Eigen::Vector3d::Zero() );
-    FrameBasePtr F2 = P->emplaceFrame(AUXILIARY,     2, Eigen::Vector3d::Zero() );
-    FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero() );
-
-    FrameBasePtr KF; // closest key-frame queried
-
-    KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8);          // before all keyframes    --> return f0
-    ASSERT_EQ(KF->id(), F1->id());                           // same id!
-
-    KF = T->closestKeyOrAuxFrameToTimeStamp(1.1);           // between keyframes       --> return F1
-    ASSERT_EQ(KF->id(), F1->id());                           // same id!
-
-    KF = T->closestKeyOrAuxFrameToTimeStamp(1.9);           // between keyframes       --> return F2
-    ASSERT_EQ(KF->id(), F2->id());                           // same id!
-
-    KF = T->closestKeyOrAuxFrameToTimeStamp(2.6);           // between keyframe and frame, but closer to frame --> return F2
-    ASSERT_EQ(KF->id(), F2->id());                           // same id!
-
-    KF = T->closestKeyOrAuxFrameToTimeStamp(3.2);           // after the last frame    --> return F2
+    KF = T->closestFrameToTimeStamp(4.2);                 // after the last frame --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 }
 
@@ -108,28 +80,29 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 
     // add F1
-    FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); // 2 non-fixed
+    FrameBasePtr F1 = P->emplaceFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 1);
+    ASSERT_EQ(T->getFrameMap().             size(), (SizeStd) 1);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
     std::cout << __LINE__ << std::endl;
 
     // add F2
-    FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
+    FrameBasePtr F2 = P->emplaceFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
+    ASSERT_EQ(T->getFrameMap().             size(), (SizeStd) 2);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
     std::cout << __LINE__ << std::endl;
 
     // add F3
-    FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero());
+    FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+                                                  //                                                              P->getDim(),
+                                                  std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 3);
+    ASSERT_EQ(T->getFrameMap().             size(), (SizeStd) 2);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F2->id());
     std::cout << __LINE__ << std::endl;
 
     N.update();
@@ -139,24 +112,23 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     // remove frames and keyframes
     F2->remove(); // KF
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
+    ASSERT_EQ(T->getFrameMap().             size(), (SizeStd) 1);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F1->id());
     std::cout << __LINE__ << std::endl;
 
     F3->remove(); // F
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 1);
+    ASSERT_EQ(T->getFrameMap().             size(), (SizeStd) 1);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F1->id());
 
     F1->remove(); // KF
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 0);
+    ASSERT_EQ(T->getFrameMap().             size(), (SizeStd) 0);
     std::cout << __LINE__ << std::endl;
 
     N.update();
@@ -165,123 +137,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 }
 
-TEST(TrajectoryBase, KeyFramesAreSorted)
-{
-    using std::make_shared;
-
-    ProblemPtr P = Problem::create("PO", 2);
-    TrajectoryBasePtr T = P->getTrajectory();
-
-    // Trajectory status:
-    //  KF1   KF2    F3      frames
-    //   1     2     3       time stamps
-    // --+-----+-----+--->   time
-
-    // add frames and keyframes in random order --> keyframes must be sorted after that
-    FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero());
-    if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero()); // non-key-frame
-    if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero());
-    if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    F3->setKey(); // set KF3
-    if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
-
-    auto F4 = P->emplaceFrame(NON_ESTIMATED, 1.5, Eigen::Vector3d::Zero());
-    // Trajectory status:
-    //  KF1   KF2   KF3     F4       frames
-    //   1     2     3     1.5       time stamps
-    // --+-----+-----+------+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F4->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
-
-    F4->setKey();
-    // Trajectory status:
-    //  KF1   KF4   KF2    KF3       frames
-    //   1    1.5    2      3        time stamps
-    // --+-----+-----+------+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
-
-    F2->setTimeStamp(4);
-    // Trajectory status:
-    //  KF1   KF4   KF3    KF2       frames
-    //   1    1.5    3      4        time stamps
-    // --+-----+-----+------+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    F4->setTimeStamp(0.5);
-    // Trajectory status:
-    //  KF4   KF2   KF3    KF2       frames
-    //  0.5    1     3      4        time stamps
-    // --+-----+-----+------+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
-
-    auto F5 = P->emplaceFrame(AUXILIARY, 1.5, Eigen::Vector3d::Zero());
-    // Trajectory status:
-    //  KF4   KF2  AuxF5  KF3   KF2       frames
-    //  0.5    1    1.5    3     4        time stamps
-    // --+-----+-----+-----+-----+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    F5->setTimeStamp(5);
-    // Trajectory status:
-    //  KF4   KF2   KF3   KF2   AuxF5     frames
-    //  0.5    1     3     4     5        time stamps
-    // --+-----+-----+-----+-----+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F5->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    auto F6 = P->emplaceFrame(NON_ESTIMATED, 6, Eigen::Vector3d::Zero());
-    // Trajectory status:
-    //  KF4   KF2   KF3   KF2   AuxF5  F6       frames
-    //  0.5    1     3     4     5     6        time stamps
-    // --+-----+-----+-----+-----+-----+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F6->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-    auto F7 = P->emplaceFrame(NON_ESTIMATED, 5.5, Eigen::Vector3d::Zero());
-    // Trajectory status:
-    //  KF4   KF2   KF3   KF2   AuxF5  F7   F6       frames
-    //  0.5    1     3     4     5     5.5   6        time stamps
-    // --+-----+-----+-----+-----+-----+-----+--->    time
-    if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()         ->id(), F7->id()); //Only auxiliary and key-frames are sorted
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
-    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
-
-}
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 1d97e2925487f9e363e1292b6ef2c2a82c29fc96..9c684c7bc0f3f30199a8d86e2ca9bf4b8aacb87f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback)
 
     ASSERT_EQ(GM->n_KF_, 0);
 
-    auto F0 = P->emplaceFrame(KEY, 0, "PO", 3, VectorXd(7) );
+    auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
     P->keyFrameCallback(F0, nullptr, 0);
 
     ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 5cbc9550cc9a751a57046807f236486f4d5cbb92..e8ffeb407df4df15c00715b21a3e86d554b38da8 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -24,7 +24,7 @@ TEST(TreeManagerSlidingWindow, make_shared)
 
     P->setTreeManager(GM);
 
-    ASSERT_EQ(P->getTreeManager(), GM);
+    EXPECT_EQ(P->getTreeManager(), GM);
 }
 
 TEST(TreeManagerSlidingWindow, createParams)
@@ -35,12 +35,12 @@ TEST(TreeManagerSlidingWindow, createParams)
 
     auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
 
     P->setTreeManager(GM);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
-    ASSERT_EQ(P->getTreeManager(), GM);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+    EXPECT_EQ(P->getTreeManager(), GM);
 }
 
 TEST(TreeManagerSlidingWindow, createParamServer)
@@ -52,12 +52,12 @@ TEST(TreeManagerSlidingWindow, createParamServer)
 
     auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
 
     P->setTreeManager(GM);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
-    ASSERT_EQ(P->getTreeManager(), GM);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+    EXPECT_EQ(P->getTreeManager(), GM);
 }
 
 TEST(TreeManagerSlidingWindow, autoConf)
@@ -67,7 +67,7 @@ TEST(TreeManagerSlidingWindow, autoConf)
 
     ProblemPtr P = Problem::autoSetup(server);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
 }
 
 TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
@@ -79,15 +79,15 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     P->applyPriorOptions(0);
 
     // FRAME 1 ----------------------------------------------------------
-    auto F1 = P->getTrajectory()->getLastKeyFrame();
-    ASSERT_TRUE(F1 != nullptr);
+    auto F1 = P->getTrajectory()->getLastFrame();
+    EXPECT_TRUE(F1 != nullptr);
 
     Vector7d state = F1->getStateVector();
     Vector7d zero_disp(state);
     Matrix6d cov = Matrix6d::Identity();
 
     // FRAME 2 ----------------------------------------------------------
-    auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
+    auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
     P->keyFrameCallback(F2, nullptr, 0);
 
     // absolute factor
@@ -100,10 +100,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 3 ----------------------------------------------------------
-    auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3,    state);
+    auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
     P->keyFrameCallback(F3, nullptr, 0);
 
     // absolute factor
@@ -116,10 +116,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 4 ----------------------------------------------------------
-    auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3,    state);
+    auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
     P->keyFrameCallback(F4, nullptr, 0);
 
     // absolute factor
@@ -132,14 +132,14 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_TRUE(C12->isRemoving()); //Virally removed
-    ASSERT_TRUE(f12->isRemoving()); //Virally removed
-    ASSERT_TRUE(F2->isFixed()); //Fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C12->isRemoving()); // Virally removed
+    EXPECT_TRUE(f12->isRemoving()); // Virally removed
+    EXPECT_TRUE(F2->isFixed()); //Fixed
 
     // FRAME 5 ----------------------------------------------------------
-    auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3,    state);
+    auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
     P->keyFrameCallback(F5, nullptr, 0);
 
     // absolute factor
@@ -152,18 +152,19 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_TRUE(C12->isRemoving()); //Virally removed
-    ASSERT_TRUE(f12->isRemoving()); //Virally removed
-    ASSERT_TRUE(F2->isRemoving());
-    ASSERT_TRUE(c2->isRemoving());
-    ASSERT_TRUE(C2->isRemoving()); //Virally removed
-    ASSERT_TRUE(f2->isRemoving()); //Virally removed
-    ASSERT_TRUE(c23->isRemoving());
-    ASSERT_TRUE(C23->isRemoving()); //Virally removed
-    ASSERT_TRUE(f23->isRemoving()); //Virally removed
-    ASSERT_TRUE(F3->isFixed()); //Fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C12->isRemoving()); // Virally removed
+    EXPECT_TRUE(f12->isRemoving()); // Virally removed
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C2->isRemoving()); // Virally removed
+    EXPECT_TRUE(f2->isRemoving()); // Virally removed
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C23->isRemoving()); // Virally removed
+    EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+    EXPECT_TRUE(F3->isFixed()); //Fixed
 }
 
 TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
@@ -175,15 +176,15 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     P->applyPriorOptions(0);
 
     // FRAME 1 (prior) ----------------------------------------------------------
-    auto F1 = P->getTrajectory()->getLastKeyFrame();
-    ASSERT_TRUE(F1 != nullptr);
+    auto F1 = P->getTrajectory()->getLastFrame();
+    EXPECT_TRUE(F1 != nullptr);
 
     Vector7d state = F1->getStateVector();
     Vector7d zero_disp(state);
     Matrix6d cov = Matrix6d::Identity();
 
     // FRAME 2 ----------------------------------------------------------
-    auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3,    state);
+    auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3,    state);
     P->keyFrameCallback(F2, nullptr, 0);
 
     // absolute factor
@@ -196,10 +197,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 3 ----------------------------------------------------------
-    auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3,    state);
+    auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
     P->keyFrameCallback(F3, nullptr, 0);
 
     // absolute factor
@@ -212,10 +213,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 4 ----------------------------------------------------------
-    auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3,    state);
+    auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
     P->keyFrameCallback(F4, nullptr, 0);
 
     // absolute factor
@@ -228,14 +229,14 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_FALSE(C12->isRemoving()); //Not virally removed
-    ASSERT_FALSE(f12->isRemoving()); //Not virally removed
-    ASSERT_FALSE(F2->isFixed()); //Not fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C12->isRemoving()); //Not virally removed
+    EXPECT_FALSE(f12->isRemoving()); //Not virally removed
+    EXPECT_FALSE(F2->isFixed()); //Not fixed
 
     // FRAME 5 ----------------------------------------------------------
-    auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3,    state);
+    auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
     P->keyFrameCallback(F5, nullptr, 0);
 
     // absolute factor
@@ -248,18 +249,18 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_TRUE(C12->isRemoving());
-    ASSERT_TRUE(f12->isRemoving());
-    ASSERT_TRUE(F2->isRemoving());
-    ASSERT_TRUE(c2->isRemoving());
-    ASSERT_TRUE(C2->isRemoving());
-    ASSERT_TRUE(f2->isRemoving());
-    ASSERT_TRUE(c23->isRemoving());
-    ASSERT_FALSE(C23->isRemoving()); //Not virally removed
-    ASSERT_FALSE(f23->isRemoving()); //Not virally removed
-    ASSERT_FALSE(F3->isFixed()); //Not fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C23->isRemoving()); //Not virally removed
+    EXPECT_FALSE(f23->isRemoving()); //Not virally removed
+    EXPECT_FALSE(F3->isFixed()); //Not fixed
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa1414aafe81283cbc931b20498e3b737b13b533
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -0,0 +1,1066 @@
+#include "core/utils/utils_gtest.h"
+
+
+#include "core/problem/problem.h"
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/capture/capture_void.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_odom_3d.h"
+#include "core/factor/factor_pose_3d.h"
+#include "core/solver/factory_solver.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManagerSlidingWindowDualRate, make_shared)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+
+    auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+    auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM);
+
+    P->setTreeManager(GM);
+
+    EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, createParams)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+
+    auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+    auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", ParamsGM);
+
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+
+    P->setTreeManager(GM);
+
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+    EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, createParamServer)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+
+    ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+    ParamsServer server = ParamsServer(parser.getParams());
+
+    auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", server);
+
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+
+    P->setTreeManager(GM);
+
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+    EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, autoConf)
+{
+    ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+    ParamsServer server = ParamsServer(parser.getParams());
+
+    ProblemPtr P = Problem::autoSetup(server);
+
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
+{
+    /* sliding window dual rate:
+     *     n_frames: 5
+     *     n_frames_recent: 3
+     *     rate_old_frames: 2
+     */
+
+    ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+    ParamsServer server = ParamsServer(parser.getParams());
+
+    ProblemPtr P = Problem::autoSetup(server);
+    P->applyPriorOptions(0);
+
+    /* FRAME 1 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (  )   (  )(  )(F1)
+     */
+    auto F1 = P->getTrajectory()->getLastFrame();
+    ASSERT_TRUE(F1 != nullptr);
+    ASSERT_FALSE(F1->getCaptureList().empty());
+    auto C1 = F1->getCaptureList().front();
+    ASSERT_FALSE(C1->getFeatureList().empty());
+    auto f1 = C1->getFeatureList().front();
+    ASSERT_FALSE(f1->getFactorList().empty());
+    auto c1 = f1->getFactorList().front();
+
+    Vector7d state = F1->getStateVector();
+    Vector7d zero_disp(state);
+    Matrix6d cov = Matrix6d::Identity();
+
+    // Check no frame removed
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+
+    /* FRAME 2 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (  )   (  )(F1)(F2)
+     */
+    auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+    P->keyFrameCallback(F2, nullptr, 0);
+
+    // absolute factor
+    auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+    auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+    auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+    // displacement
+    auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+    auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+    auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+
+    // Check no frame removed
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F2->isRemoving());
+    EXPECT_FALSE(c2->isRemoving());
+    EXPECT_FALSE(C2->isRemoving());
+    EXPECT_FALSE(f2->isRemoving());
+    EXPECT_FALSE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving());
+    EXPECT_FALSE(f12->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+
+    /* FRAME 3 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (  )   (F1)(F2)(F3)
+     */
+    auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
+    P->keyFrameCallback(F3, nullptr, 0);
+
+    // absolute factor
+    auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+    auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+    auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+    // displacement
+    auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+    auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+    auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+
+    // Check no frame removed
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F2->isRemoving());
+    EXPECT_FALSE(c2->isRemoving());
+    EXPECT_FALSE(C2->isRemoving());
+    EXPECT_FALSE(f2->isRemoving());
+    EXPECT_FALSE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving());
+    EXPECT_FALSE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_FALSE(c23->isRemoving());
+    EXPECT_FALSE(C23->isRemoving());
+    EXPECT_FALSE(f23->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+
+    /* FRAME 4 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (F1)(F2)(F3)(F4)
+     */
+    auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
+    P->keyFrameCallback(F4, nullptr, 0);
+
+    // absolute factor
+    auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+    auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+    auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+    // displacement
+    auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+    auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+    auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F2->isRemoving());
+    EXPECT_FALSE(c2->isRemoving());
+    EXPECT_FALSE(C2->isRemoving());
+    EXPECT_FALSE(f2->isRemoving());
+    EXPECT_FALSE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving());
+    EXPECT_FALSE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_FALSE(c23->isRemoving());
+    EXPECT_FALSE(C23->isRemoving());
+    EXPECT_FALSE(f23->isRemoving());
+
+    EXPECT_FALSE(F4->isRemoving());
+    EXPECT_FALSE(c4->isRemoving());
+    EXPECT_FALSE(C4->isRemoving());
+    EXPECT_FALSE(f4->isRemoving());
+    EXPECT_FALSE(c34->isRemoving());
+    EXPECT_FALSE(C34->isRemoving());
+    EXPECT_FALSE(f34->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+
+    /* FRAME 5 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (F1)   (F3)(F4)(F5)
+     */
+    auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
+    P->keyFrameCallback(F5, nullptr, 0);
+
+    // absolute factor
+    auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+    auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+    auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+    // displacement
+    auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+    auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+    auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C23->isRemoving()); // Virally removed
+    EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+    EXPECT_FALSE(F4->isRemoving());
+    EXPECT_FALSE(c4->isRemoving());
+    EXPECT_FALSE(C4->isRemoving());
+    EXPECT_FALSE(f4->isRemoving());
+    EXPECT_FALSE(c34->isRemoving());
+    EXPECT_FALSE(C34->isRemoving());
+    EXPECT_FALSE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_FALSE(c45->isRemoving());
+    EXPECT_FALSE(C45->isRemoving());
+    EXPECT_FALSE(f45->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+
+    /* FRAME 6 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (F1)   (F3)(F4)(F5)(F6)
+     */
+    auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3,    state);
+    P->keyFrameCallback(F6, nullptr, 0);
+
+    // absolute factor
+    auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+    auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov);
+    auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false);
+    // displacement
+    auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+    auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
+    auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C23->isRemoving()); // Virally removed
+    EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+    EXPECT_FALSE(F4->isRemoving());
+    EXPECT_FALSE(c4->isRemoving());
+    EXPECT_FALSE(C4->isRemoving());
+    EXPECT_FALSE(f4->isRemoving());
+    EXPECT_FALSE(c34->isRemoving());
+    EXPECT_FALSE(C34->isRemoving());
+    EXPECT_FALSE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_FALSE(c45->isRemoving());
+    EXPECT_FALSE(C45->isRemoving());
+    EXPECT_FALSE(f45->isRemoving());
+
+    EXPECT_FALSE(F6->isRemoving());
+    EXPECT_FALSE(c6->isRemoving());
+    EXPECT_FALSE(C6->isRemoving());
+    EXPECT_FALSE(f6->isRemoving());
+    EXPECT_FALSE(c56->isRemoving());
+    EXPECT_FALSE(C56->isRemoving());
+    EXPECT_FALSE(f56->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+    EXPECT_FALSE(F6->isFixed());
+
+    /* FRAME 7 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (F1)   (F3)   (F5)(F6)(F7)
+     */
+    auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3,    state);
+    P->keyFrameCallback(F7, nullptr, 0);
+
+    // absolute factor
+    auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+    auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov);
+    auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false);
+    // displacement
+    auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+    auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
+    auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C23->isRemoving()); // Virally removed
+    EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+    EXPECT_TRUE(F4->isRemoving());
+    EXPECT_TRUE(c4->isRemoving());
+    EXPECT_TRUE(C4->isRemoving());
+    EXPECT_TRUE(f4->isRemoving());
+    EXPECT_TRUE(c34->isRemoving());
+    EXPECT_TRUE(C34->isRemoving());
+    EXPECT_TRUE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C45->isRemoving()); // Virally removed
+    EXPECT_TRUE(f45->isRemoving()); // Virally removed
+
+    EXPECT_FALSE(F6->isRemoving());
+    EXPECT_FALSE(c6->isRemoving());
+    EXPECT_FALSE(C6->isRemoving());
+    EXPECT_FALSE(f6->isRemoving());
+    EXPECT_FALSE(c56->isRemoving());
+    EXPECT_FALSE(C56->isRemoving());
+    EXPECT_FALSE(f56->isRemoving());
+
+    EXPECT_FALSE(F7->isRemoving());
+    EXPECT_FALSE(c7->isRemoving());
+    EXPECT_FALSE(C7->isRemoving());
+    EXPECT_FALSE(f7->isRemoving());
+    EXPECT_FALSE(c67->isRemoving());
+    EXPECT_FALSE(C67->isRemoving());
+    EXPECT_FALSE(f67->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+    EXPECT_FALSE(F6->isFixed());
+    EXPECT_FALSE(F7->isFixed());
+
+    /* FRAME 8 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (F3)   (F5)(F6)(F7)(F8)
+     */
+    auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3,    state);
+    P->keyFrameCallback(F8, nullptr, 0);
+
+    // absolute factor
+    auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+    auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov);
+    auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false);
+    // displacement
+    auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+    auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
+    auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false);
+
+    // Checks
+    EXPECT_TRUE(F1->isRemoving()); // First frame removed
+    EXPECT_TRUE(c1->isRemoving());
+    EXPECT_TRUE(C1->isRemoving());
+    EXPECT_TRUE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C23->isRemoving()); // Virally removed
+    EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+    EXPECT_TRUE(F4->isRemoving());
+    EXPECT_TRUE(c4->isRemoving());
+    EXPECT_TRUE(C4->isRemoving());
+    EXPECT_TRUE(f4->isRemoving());
+    EXPECT_TRUE(c34->isRemoving());
+    EXPECT_TRUE(C34->isRemoving());
+    EXPECT_TRUE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C45->isRemoving()); // Virally removed
+    EXPECT_TRUE(f45->isRemoving()); // Virally removed
+
+    EXPECT_FALSE(F6->isRemoving());
+    EXPECT_FALSE(c6->isRemoving());
+    EXPECT_FALSE(C6->isRemoving());
+    EXPECT_FALSE(f6->isRemoving());
+    EXPECT_FALSE(c56->isRemoving());
+    EXPECT_FALSE(C56->isRemoving());
+    EXPECT_FALSE(f56->isRemoving());
+
+    EXPECT_FALSE(F7->isRemoving());
+    EXPECT_FALSE(c7->isRemoving());
+    EXPECT_FALSE(C7->isRemoving());
+    EXPECT_FALSE(f7->isRemoving());
+    EXPECT_FALSE(c67->isRemoving());
+    EXPECT_FALSE(C67->isRemoving());
+    EXPECT_FALSE(f67->isRemoving());
+
+    EXPECT_FALSE(F8->isRemoving());
+    EXPECT_FALSE(c8->isRemoving());
+    EXPECT_FALSE(C8->isRemoving());
+    EXPECT_FALSE(f8->isRemoving());
+    EXPECT_FALSE(c78->isRemoving());
+    EXPECT_FALSE(C78->isRemoving());
+    EXPECT_FALSE(f78->isRemoving());
+
+    EXPECT_TRUE(F3->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+    EXPECT_FALSE(F6->isFixed());
+    EXPECT_FALSE(F7->isFixed());
+    EXPECT_FALSE(F8->isFixed());
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
+{
+    /* sliding window dual rate:
+     *     n_frames: 5
+     *     n_frames_recent: 3
+     *     rate_old_frames: 2
+     */
+
+    ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", wolf_root);
+    ParamsServer server = ParamsServer(parser.getParams());
+
+    ProblemPtr P = Problem::autoSetup(server);
+    P->applyPriorOptions(0);
+
+    /* FRAME 1 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (  )   (  )(  )(F1)
+     */
+    auto F1 = P->getTrajectory()->getLastFrame();
+    ASSERT_TRUE(F1 != nullptr);
+    ASSERT_FALSE(F1->getCaptureList().empty());
+    auto C1 = F1->getCaptureList().front();
+    ASSERT_FALSE(C1->getFeatureList().empty());
+    auto f1 = C1->getFeatureList().front();
+    ASSERT_FALSE(f1->getFactorList().empty());
+    auto c1 = f1->getFactorList().front();
+
+    Vector7d state = F1->getStateVector();
+    Vector7d zero_disp(state);
+    Matrix6d cov = Matrix6d::Identity();
+
+    // Check no frame removed
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+
+    /* FRAME 2 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (  )   (  )(F1)(F2)
+     */
+    auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+    P->keyFrameCallback(F2, nullptr, 0);
+
+    // absolute factor
+    auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+    auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+    auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+    // displacement
+    auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+    auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+    auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+
+    // Check no frame removed
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F2->isRemoving());
+    EXPECT_FALSE(c2->isRemoving());
+    EXPECT_FALSE(C2->isRemoving());
+    EXPECT_FALSE(f2->isRemoving());
+    EXPECT_FALSE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving());
+    EXPECT_FALSE(f12->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+
+    /* FRAME 3 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (  )   (F1)(F2)(F3)
+     */
+    auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
+    P->keyFrameCallback(F3, nullptr, 0);
+
+    // absolute factor
+    auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+    auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+    auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+    // displacement
+    auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+    auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+    auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+
+    // Check no frame removed
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F2->isRemoving());
+    EXPECT_FALSE(c2->isRemoving());
+    EXPECT_FALSE(C2->isRemoving());
+    EXPECT_FALSE(f2->isRemoving());
+    EXPECT_FALSE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving());
+    EXPECT_FALSE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_FALSE(c23->isRemoving());
+    EXPECT_FALSE(C23->isRemoving());
+    EXPECT_FALSE(f23->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+
+    /* FRAME 4 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (F1)(F2)(F3)(F4)
+     */
+    auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
+    P->keyFrameCallback(F4, nullptr, 0);
+
+    // absolute factor
+    auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+    auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+    auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+    // displacement
+    auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+    auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+    auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_FALSE(F2->isRemoving());
+    EXPECT_FALSE(c2->isRemoving());
+    EXPECT_FALSE(C2->isRemoving());
+    EXPECT_FALSE(f2->isRemoving());
+    EXPECT_FALSE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving());
+    EXPECT_FALSE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_FALSE(c23->isRemoving());
+    EXPECT_FALSE(C23->isRemoving());
+    EXPECT_FALSE(f23->isRemoving());
+
+    EXPECT_FALSE(F4->isRemoving());
+    EXPECT_FALSE(c4->isRemoving());
+    EXPECT_FALSE(C4->isRemoving());
+    EXPECT_FALSE(f4->isRemoving());
+    EXPECT_FALSE(c34->isRemoving());
+    EXPECT_FALSE(C34->isRemoving());
+    EXPECT_FALSE(f34->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+
+    /* FRAME 5 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (  )   (F1)   (F3)(F4)(F5)
+     */
+    auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
+    P->keyFrameCallback(F5, nullptr, 0);
+
+    // absolute factor
+    auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+    auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+    auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+    // displacement
+    auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+    auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+    auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+    EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+    EXPECT_FALSE(F4->isRemoving());
+    EXPECT_FALSE(c4->isRemoving());
+    EXPECT_FALSE(C4->isRemoving());
+    EXPECT_FALSE(f4->isRemoving());
+    EXPECT_FALSE(c34->isRemoving());
+    EXPECT_FALSE(C34->isRemoving());
+    EXPECT_FALSE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_FALSE(c45->isRemoving());
+    EXPECT_FALSE(C45->isRemoving());
+    EXPECT_FALSE(f45->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+
+    /* FRAME 6 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (F1)   (F3)(F4)(F5)(F6)
+     */
+    auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3,    state);
+    P->keyFrameCallback(F6, nullptr, 0);
+
+    // absolute factor
+    auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+    auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov);
+    auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false);
+    // displacement
+    auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+    auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
+    auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+    EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+    EXPECT_FALSE(F4->isRemoving());
+    EXPECT_FALSE(c4->isRemoving());
+    EXPECT_FALSE(C4->isRemoving());
+    EXPECT_FALSE(f4->isRemoving());
+    EXPECT_FALSE(c34->isRemoving());
+    EXPECT_FALSE(C34->isRemoving());
+    EXPECT_FALSE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_FALSE(c45->isRemoving());
+    EXPECT_FALSE(C45->isRemoving());
+    EXPECT_FALSE(f45->isRemoving());
+
+    EXPECT_FALSE(F6->isRemoving());
+    EXPECT_FALSE(c6->isRemoving());
+    EXPECT_FALSE(C6->isRemoving());
+    EXPECT_FALSE(f6->isRemoving());
+    EXPECT_FALSE(c56->isRemoving());
+    EXPECT_FALSE(C56->isRemoving());
+    EXPECT_FALSE(f56->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+    EXPECT_FALSE(F6->isFixed());
+
+    /* FRAME 7 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (F1)   (F3)   (F5)(F6)(F7)
+     */
+    auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3,    state);
+    P->keyFrameCallback(F7, nullptr, 0);
+
+    // absolute factor
+    auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+    auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov);
+    auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false);
+    // displacement
+    auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+    auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
+    auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false);
+
+    // Checks
+    EXPECT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(c1->isRemoving());
+    EXPECT_FALSE(C1->isRemoving());
+    EXPECT_FALSE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+    EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+    EXPECT_TRUE(F4->isRemoving());
+    EXPECT_TRUE(c4->isRemoving());
+    EXPECT_TRUE(C4->isRemoving());
+    EXPECT_TRUE(f4->isRemoving());
+    EXPECT_TRUE(c34->isRemoving());
+    EXPECT_TRUE(C34->isRemoving());
+    EXPECT_TRUE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C45->isRemoving()); // Not virally removed
+    EXPECT_FALSE(f45->isRemoving()); // Not virally removed
+
+    EXPECT_FALSE(F6->isRemoving());
+    EXPECT_FALSE(c6->isRemoving());
+    EXPECT_FALSE(C6->isRemoving());
+    EXPECT_FALSE(f6->isRemoving());
+    EXPECT_FALSE(c56->isRemoving());
+    EXPECT_FALSE(C56->isRemoving());
+    EXPECT_FALSE(f56->isRemoving());
+
+    EXPECT_FALSE(F7->isRemoving());
+    EXPECT_FALSE(c7->isRemoving());
+    EXPECT_FALSE(C7->isRemoving());
+    EXPECT_FALSE(f7->isRemoving());
+    EXPECT_FALSE(c67->isRemoving());
+    EXPECT_FALSE(C67->isRemoving());
+    EXPECT_FALSE(f67->isRemoving());
+
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+    EXPECT_FALSE(F6->isFixed());
+    EXPECT_FALSE(F7->isFixed());
+
+    /* FRAME 8 ----------------------------------------------------------
+     *
+     * Sliding window:
+     * (F3)   (F5)(F6)(F7)(F8)
+     */
+    auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3,    state);
+    P->keyFrameCallback(F8, nullptr, 0);
+
+    // absolute factor
+    auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+    auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov);
+    auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false);
+    // displacement
+    auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+    auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
+    auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false);
+
+    // Checks
+    EXPECT_TRUE(F1->isRemoving()); // First frame removed
+    EXPECT_TRUE(c1->isRemoving());
+    EXPECT_TRUE(C1->isRemoving());
+    EXPECT_TRUE(f1->isRemoving());
+
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+
+    EXPECT_FALSE(F3->isRemoving());
+    EXPECT_FALSE(c3->isRemoving());
+    EXPECT_FALSE(C3->isRemoving());
+    EXPECT_FALSE(f3->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C23->isRemoving()); // Not virally removed
+    EXPECT_FALSE(f23->isRemoving()); // Not virally removed
+
+    EXPECT_TRUE(F4->isRemoving());
+    EXPECT_TRUE(c4->isRemoving());
+    EXPECT_TRUE(C4->isRemoving());
+    EXPECT_TRUE(f4->isRemoving());
+    EXPECT_TRUE(c34->isRemoving());
+    EXPECT_TRUE(C34->isRemoving());
+    EXPECT_TRUE(f34->isRemoving());
+
+    EXPECT_FALSE(F5->isRemoving());
+    EXPECT_FALSE(c5->isRemoving());
+    EXPECT_FALSE(C5->isRemoving());
+    EXPECT_FALSE(f5->isRemoving());
+    EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_FALSE(C45->isRemoving()); // Not virally removed
+    EXPECT_FALSE(f45->isRemoving()); // Not virally removed
+
+    EXPECT_FALSE(F6->isRemoving());
+    EXPECT_FALSE(c6->isRemoving());
+    EXPECT_FALSE(C6->isRemoving());
+    EXPECT_FALSE(f6->isRemoving());
+    EXPECT_FALSE(c56->isRemoving());
+    EXPECT_FALSE(C56->isRemoving());
+    EXPECT_FALSE(f56->isRemoving());
+
+    EXPECT_FALSE(F7->isRemoving());
+    EXPECT_FALSE(c7->isRemoving());
+    EXPECT_FALSE(C7->isRemoving());
+    EXPECT_FALSE(f7->isRemoving());
+    EXPECT_FALSE(c67->isRemoving());
+    EXPECT_FALSE(C67->isRemoving());
+    EXPECT_FALSE(f67->isRemoving());
+
+    EXPECT_FALSE(F8->isRemoving());
+    EXPECT_FALSE(c8->isRemoving());
+    EXPECT_FALSE(C8->isRemoving());
+    EXPECT_FALSE(f8->isRemoving());
+    EXPECT_FALSE(c78->isRemoving());
+    EXPECT_FALSE(C78->isRemoving());
+    EXPECT_FALSE(f78->isRemoving());
+
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F5->isFixed());
+    EXPECT_FALSE(F6->isFixed());
+    EXPECT_FALSE(F7->isFixed());
+    EXPECT_FALSE(F8->isFixed());
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
+{
+    // SLIDING WINDOW DUAL RATE
+    ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", wolf_root);
+    ParamsServer server = ParamsServer(parser.getParams());
+    ProblemPtr problem = Problem::autoSetup(server);
+    SolverManagerPtr solver = FactorySolver::create("SolverCeres", problem, server);
+
+    // BASELINE
+    ParserYaml parser_bl = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml", wolf_root);
+    ParamsServer server_bl = ParamsServer(parser_bl.getParams());
+    ProblemPtr problem_bl = Problem::autoSetup(server_bl);
+    SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server);
+
+    // aux variables
+    Vector7d data;
+    Matrix6d data_cov = Matrix6d::Identity();
+    TimeStamp t(0.0);
+    double dt = 1;
+    CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t,
+                                                               problem->getSensor("odom"),
+                                                               data,
+                                                               data_cov);
+    CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t,
+                                                                  problem_bl->getSensor("odom"),
+                                                                  data,
+                                                                  data_cov);
+
+    // START MOTION CAPTURES
+    for (int i = 0; i<20; i++)
+    {
+        t += dt;
+        WOLF_INFO("-------------------------- t: ", t);
+
+        // random movement
+        data.setRandom(); data.tail<4>().normalize();
+
+        // sliding window process
+        capture->setTimeStamp(t);
+        capture->setData(data);
+        capture->process();
+
+        // baseline process
+        capture_bl->setTimeStamp(t);
+        capture_bl->setData(data);
+        capture_bl->process();
+
+        WOLF_INFO("state: ", problem->getState().vector("PO").transpose());
+
+        ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+                             problem_bl->getState().vector("PO").transpose(),
+                             1e-12);
+
+        // Solve
+        solver->solve();
+        solver_bl->solve();
+
+        ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+                             problem_bl->getState().vector("PO").transpose(),
+                             1e-12);
+
+        ASSERT_TRUE(problem->check());
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 96ad94f72af7e8de1b9200a8e5ea44e3bc952a85..22242febd640b616bebe4ab0410867b0e373a5be 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -4,8 +4,6 @@ config:
     dimension: 3
     prior:
       mode: "factor"
-      # state: [0,0,0,0,0,0,1]
-      # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
       $state:
         P: [0,0,0]
         O: [0,0,0,1]
@@ -15,8 +13,8 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindow"
-      n_key_frames: 3
-      fix_first_key_frame: true
+      n_frames: 3
+      fix_first_frame: true
       viral_remove_empty_parent: true
   sensors: 
     -
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index 9ae33a2af5ab0695bd44948d33d2f9aa104cbc74..add5eff760050fecbde63c827bb75e77c531f86e 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -4,8 +4,6 @@ config:
     dimension: 3
     prior:
       mode: "factor"
-      # state: [0,0,0,0,0,0,1]
-      # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
       $state:
         P: [0,0,0]
         O: [0,0,0,1]
@@ -15,8 +13,8 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindow"
-      n_key_frames: 3
-      fix_first_key_frame: false
+      n_frames: 3
+      fix_first_frame: false
       viral_remove_empty_parent: false
   sensors: 
     -
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..32ef665e0a6d92b94e9342d22c5e4ca9952613f1
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -0,0 +1,20 @@
+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager:
+      type: "TreeManagerSlidingWindowDualRate"
+      n_frames: 5
+      n_frames_recent: 3
+      rate_old_frames: 2
+      fix_first_frame: true
+      viral_remove_empty_parent: true
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6b4795fb3ad70c77b02c9cffc74b964abfe64141
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -0,0 +1,20 @@
+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager:
+      type: "TreeManagerSlidingWindowDualRate"
+      n_frames: 5
+      n_frames_recent: 3
+      rate_old_frames: 2
+      fix_first_frame: false
+      viral_remove_empty_parent: false
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b59022518aaf931890368185fb9b489611cda257
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -0,0 +1,55 @@
+config:
+  solver:
+    period: 1
+    verbose: 2
+    update_immediately: false
+    max_num_iterations: 10
+    n_threads: 2
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager:
+      type: "TreeManagerSlidingWindowDualRate"
+      n_frames: 5
+      n_frames_recent: 3
+      rate_old_frames: 2
+      fix_first_frame: true
+      viral_remove_empty_parent: true
+  sensors: 
+    -
+      type: "SensorOdom3d"
+      name: "odom"
+      plugin: "core"
+      k_disp_to_disp: 0.1
+      k_disp_to_rot: 0.1
+      k_rot_to_rot: 0.1 
+      min_disp_var: 0.1 
+      min_rot_var: 0.1
+      extrinsic:
+        pose: [1,2,3,0,0,0,1]
+  processors:
+    -
+      type: "ProcessorOdom3d"
+      name: "my_proc_odom3d"
+      sensor_name: "odom"
+      plugin: "core"
+      apply_loss_function: false
+      time_tolerance:         0.01  # seconds
+      keyframe_vote:
+        voting_active:        true
+        voting_aux_active:    false
+        max_time_span:          0.2   # seconds
+        max_buff_length:        10    # motion deltas
+        dist_traveled:          10   # meters
+        angle_turned:           3.1   # radians (1 rad approx 57 deg, approx 60 deg)
+      
+      unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eba780939b0e4521ba7a5d571ed47d63749adc6e
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -0,0 +1,49 @@
+config:
+  solver:
+    period: 1
+    verbose: 2
+    update_immediately: false
+    max_num_iterations: 10
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager: 
+      type: "None"
+  sensors: 
+    -
+      type: "SensorOdom3d"
+      name: "odom"
+      plugin: "core"
+      k_disp_to_disp: 0.1
+      k_disp_to_rot: 0.1
+      k_rot_to_rot: 0.1 
+      min_disp_var: 0.1 
+      min_rot_var: 0.1
+      extrinsic:
+        pose: [1,2,3,0,0,0,1]
+  processors:
+    -
+      type: "ProcessorOdom3d"
+      name: "my_proc_odom3d"
+      sensor_name: "odom"
+      plugin: "core"
+      apply_loss_function: false
+      time_tolerance:         0.01  # seconds
+      keyframe_vote:
+        voting_active:        true
+        voting_aux_active:    false
+        max_time_span:          0.2   # seconds
+        max_buff_length:        10    # motion deltas
+        dist_traveled:          10   # meters
+        angle_turned:           3.1   # radians (1 rad approx 57 deg, approx 60 deg)
+      
+      unmeasured_perturbation_std: 0.00111
diff --git a/wolf_scripts/wolf_uninstall.sh b/wolf_scripts/wolf_uninstall.sh
index b381616ecf9c37423d5cd11e18bb3f5502cec641..ff022e0b25ebc31bc0d7d0e50124569896c8f6ba 100755
--- a/wolf_scripts/wolf_uninstall.sh
+++ b/wolf_scripts/wolf_uninstall.sh
@@ -1,20 +1,42 @@
 #!/bin/bash
 
-echo “=”
-echo “====================== UNINSTALLING WOLF ======================”
-echo “=”
+if [ -z "$1" ]
+then
+    echo “=”
+    echo “====================== UNINSTALLING WOLF ======================”
+    echo “=”
 
-echo "cd /usr/local/include/iri-algorithms/"
-cd /usr/local/include/iri-algorithms/
-echo "sudo rm -rf wolf"
-sudo rm -rf wolf
+    echo "cd /usr/local/include/iri-algorithms/"
+    cd /usr/local/include/iri-algorithms/
+    echo "sudo rm -rf wolf"
+    sudo rm -rf wolf
 
-echo "cd /usr/local/lib/iri-algorithms/"
-echo "sudo rm libwolf*.*"
-cd /usr/local/lib/iri-algorithms/
-sudo rm libwolf*.*
+    echo "cd /usr/local/lib/iri-algorithms/"
+    echo "sudo rm libwolf*.*"
+    cd /usr/local/lib/iri-algorithms/
+    sudo rm libwolf*.*
 
-echo "cd /usr/local/lib/cmake/"
-echo "sudo rm -rf wolf*"
-cd /usr/local/lib/cmake/
-sudo rm -rf wolf*
\ No newline at end of file
+    echo "cd /usr/local/lib/cmake/"
+    echo "sudo rm -rf wolf*"
+    cd /usr/local/lib/cmake/
+    sudo rm -rf wolf*
+else
+    echo “=”
+    echo “====================== UNINSTALLING PLUGIN $1 ======================”
+    echo “=”
+
+    echo "cd /usr/local/include/iri-algorithms/wolf"
+    cd /usr/local/include/iri-algorithms/wolf/
+    echo "sudo rm -rf plugin_$1"
+    sudo rm -rf plugin_$1
+
+    echo "cd /usr/local/lib/iri-algorithms/"
+    echo "sudo rm libwolf$1.*"
+    cd /usr/local/lib/iri-algorithms/
+    sudo rm libwolf$1.*
+
+    echo "cd /usr/local/lib/cmake/"
+    echo "sudo rm -rf wolf$1"
+    cd /usr/local/lib/cmake/
+    sudo rm -rf wolf$1
+fi