diff --git a/CMakeLists.txt b/CMakeLists.txt
index b6d381755addeead6f60c35b42fe1526eda0af4f..c3c6182415cba0d9eec234f294bed49381be7c3e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -97,16 +97,16 @@ ENDIF()
 
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
-# option(BUILD_EXAMPLES "Build examples" OFF)
+# option(BUILD_DEMOS "Build examples" OFF)
 set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
+set(BUILD_DEMOS false)
 
 # Does this has any other interest
 # but for the examples ?
 # yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
+IF(BUILD_DEMOS OR BUILD_TESTS)
   set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
+ENDIF(BUILD_DEMOS OR BUILD_TESTS)
 
 
 #find dependencies.
@@ -115,57 +115,10 @@ FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
 
 FIND_PACKAGE(Threads REQUIRED)
 
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
-    MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
-	FIND_PACKAGE(GLUT REQUIRED)
-	FIND_PACKAGE(pose_state_time REQUIRED)
-  MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
-  MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
+FIND_PACKAGE(Ceres REQUIRED) #Ceres is not required
 
 # YAML with yaml-cpp
 FIND_PACKAGE(YamlCpp REQUIRED)
-IF(YAMLCPP_FOUND)
-    MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
-    MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
-    MESSAGE("glog Library FOUND: glog related sources will be built.")
-    MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
-    MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
-    MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
-    Suitesparse_INCLUDE_DIRS
-    NAMES SuiteSparse_config.h
-    PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
-   SET(Suitesparse_FOUND TRUE)
-   MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
-   SET(Suitesparse_FOUND FALSE)
-   MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
@@ -184,10 +137,11 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
 message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
 message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
+
 # include spdlog (logging library)
 FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
 IF (SPDLOG_INCLUDE_DIR)
-  INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
+  # INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
   MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
 ELSE (SPDLOG_INCLUDE_DIR)
  MESSAGE(FATAL_ERROR "Could not find spdlog")
@@ -196,32 +150,10 @@ ENDIF (SPDLOG_INCLUDE_DIR)
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES("include")
 INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-
-IF(Ceres_FOUND)
-    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
-    INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-# cereal
-IF(cereal_FOUND)
-    INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
-    INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(GLOG_FOUND)
-INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
+INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 
 #HEADERS
 
-
 SET(HDRS_COMMON
   include/core/common/factory.h
   include/core/common/node_base.h
@@ -288,6 +220,7 @@ SET(HDRS_FACTOR
   include/core/factor/factor_block_absolute.h
   include/core/factor/factor_diff_drive.h
   include/core/factor/factor_feature_dummy.h
+  include/core/factor/factor_landmark_dummy.h
   include/core/factor/factor_odom_2D.h
   include/core/factor/factor_odom_2D_analytic.h
   include/core/factor/factor_odom_3D.h
@@ -327,7 +260,7 @@ SET(HDRS_PROCESSOR
   include/core/processor/processor_tracker_feature.h
   include/core/processor/processor_tracker_feature_dummy.h
   include/core/processor/processor_tracker_landmark.h
-  # include/core/processor/processor_tracker_landmark_dummy.h
+  include/core/processor/processor_tracker_landmark_dummy.h
   include/core/processor/track_matrix.h
   )
 SET(HDRS_SENSOR
@@ -423,7 +356,7 @@ SET(SRCS_PROCESSOR
   src/processor/processor_tracker_feature.cpp
   src/processor/processor_tracker_feature_dummy.cpp
   src/processor/processor_tracker_landmark.cpp
-  # src/processor/processor_tracker_landmark_dummy.cpp
+  src/processor/processor_tracker_landmark_dummy.cpp
   src/processor/track_matrix.cpp
   )
 SET(SRCS_SENSOR
@@ -514,23 +447,21 @@ IF (Ceres_FOUND)
     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
 ENDIF(Ceres_FOUND)
 
-IF (GLOG_FOUND)
-    TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
+# IF (GLOG_FOUND)
+#     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
+# ENDIF (GLOG_FOUND)
 #check if this is done correctly
 
-IF (GLOG_FOUND)
-    IF(BUILD_TESTS)
-        MESSAGE("Building tests.")
-        add_subdirectory(test)
-    ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
+IF(BUILD_TESTS)
+  MESSAGE("Building tests.")
+  add_subdirectory(test)
+ENDIF(BUILD_TESTS)
 
-IF(BUILD_EXAMPLES)
-  #Build examples
+IF(BUILD_DEMOS)
+  #Build demos
   MESSAGE("Building demos.")
   ADD_SUBDIRECTORY(demos)
-ENDIF(BUILD_EXAMPLES)
+ENDIF(BUILD_DEMOS)
 
 
 #install library
diff --git a/PluginsInfo.md b/PluginsInfo.md
index 822109172503bf13012fb4a5b333459f7a1b8e78..33e19f05fa1e96f0699f16b16fd4e13fc6a80240 100644
--- a/PluginsInfo.md
+++ b/PluginsInfo.md
@@ -5,8 +5,8 @@
 ## Installing wolf(core)
 
 ```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/core.git
-cd core
+git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+cd wolf 
 mkdir build & cd build
 cmake ..
 make
@@ -32,8 +32,8 @@ If you want to use the core, you just need to have it installed and in your CMak
 `find_package(wolf REQUIRED)`.
 
 If wolf is indeed installed, this will define two variables
-`${wolf_INCLUDE_DIR}` which will contain the path to the wolf include directory
-and `${wolf_LIBRARY}` which will contain the path to the wolf library.
+`${wolf_INCLUDE_DIRS}` which will contain the path to the wolf include directory
+and `${wolf_LIBRARIES}` which will contain the path to the wolf library.
 
 ## Using wolf plugins
 
@@ -41,13 +41,13 @@ If you also want to use some wolf plugin, you just follow the same procedure, ch
 `find_package(wolf<plugin name> REQUIRED)`.
 
 If the pluging is indeed installed, this will define two variables
-`${wolf<plugin name>_INCLUDE_DIR}` which will contain the path to the plugin's include directory
-and `${wolf<plugin name>_LIBRARY}` which will contain the path to the plugin's library.
+`${wolf<plugin name>_INCLUDE_DIRS}` which will contain the path to the plugin's include directory
+and `${wolf<plugin name>_LIBRARIES}` which will contain the path to the plugin's library.
 
-As an example, suppose that we want to use the _vision_ plugin. First, we will clone and install it
+As an example, suppose that we want to use the _laser_ plugin. First, we will clone and install it
 ```
-git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git
-cd vision
+git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git
+cd laser 
 mkdir build && cd build
 cmake ..
 make
@@ -56,15 +56,12 @@ sudo make install
 
 Then, in the CMakeLists.txt of the application we are developing we will put the following line
 ```
-find_package(wolfvision REQUIRED)
+find_package(wolflaser REQUIRED)
 ```
 if the plugin has been correctly installed, and thus find_package succeeds, then it will define two variables
 
-- ${wolfvision_INCLUDE_DIR} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_vision`
-- ${wolfvision_LIBRARY} which is the path to the compiled (& linked) library. It should have the value `/usr/local/lib/iri-algorithms/libwolfvision.so`
-
-**IMPORTANT NOTE** For the time being, each plugin is only responsible for finding their own includes and library. This means that for instance wolfvision, which obviously requires
-the _core_ plugin, will not find its own dependencies. It is the responsibility of the programmer to do `find_package(wolf REQUIRED)` to get the includes and library. In the future this will change and each plugin will be responsible for finding all the necessary dependencies.
+- ${wolflaser_INCLUDE_DIRS} which is the path to the includes. It should have the value `/usr/local/include/iri-algorithms/wolf/plugin_laser;/usr/local/include/iri-algorithms` 
+- ${wolfvision_LIBRARIES} which is the path to the required libraries. It should have the value `/usr/local/lib/iri-algorithms/libwolflaser.so;/usr/local/lib/iri-algorithms/liblaser_scan_utils.so` 
 
 # Creating your plugin
 
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
index a72b121bde57c392cf9f13fe4a5ed73e309ddf98..e7d4a9a499c858f24af7dc8b13120931173bc9ab 100644
--- a/cmake_modules/wolfConfig.cmake
+++ b/cmake_modules/wolfConfig.cmake
@@ -1,33 +1,33 @@
 #edit the following line to add the librarie's header files
 FIND_PATH(
-    wolf_INCLUDE_DIR
+    wolf_INCLUDE_DIRS
     NAMES wolf.found
     PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-IF(wolf_INCLUDE_DIR)
-  MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIR}")
-ELSE(wolf_INCLUDE_DIR)
+IF(wolf_INCLUDE_DIRS)
+  MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}")
+ELSE(wolf_INCLUDE_DIRS)
   MESSAGE("Couldn't find wolf include dirs")
-ENDIF(wolf_INCLUDE_DIR)
+ENDIF(wolf_INCLUDE_DIRS)
 
 FIND_LIBRARY(
-    wolf_LIBRARY
+    wolf_LIBRARIES
     NAMES libwolf.so libwolf.dylib
     PATHS /usr/local/lib/iri-algorithms)
-IF(wolf_LIBRARY)
-  MESSAGE("Found wolf lib: ${wolf_LIBRARY}")
-ELSE(wolf_LIBRARY)
+IF(wolf_LIBRARIES)
+  MESSAGE("Found wolf lib: ${wolf_LIBRARIES}")
+ELSE(wolf_LIBRARIES)
   MESSAGE("Couldn't find wolf lib")
-ENDIF(wolf_LIBRARY)
+ENDIF(wolf_LIBRARIES)
 
-IF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
+IF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
    SET(wolf_FOUND TRUE)
- ELSE(wolf_INCLUDE_DIR AND wolf_LIBRARY)
+ ELSE(wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
    set(wolf_FOUND FALSE)
-ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
+ENDIF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
 
 IF (wolf_FOUND)
    IF (NOT wolf_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}")
+      MESSAGE(STATUS "Found wolf: ${wolf_LIBRARIES}")
    ENDIF (NOT wolf_FIND_QUIETLY)
 ELSE (wolf_FOUND)
    IF (wolf_FIND_REQUIRED)
@@ -38,7 +38,7 @@ ENDIF (wolf_FOUND)
 
 macro(wolf_report_not_found REASON_MSG)
   set(wolf_FOUND FALSE)
-  unset(wolf_INCLUDE_DIR)
+  unset(wolf_INCLUDE_DIRS)
   unset(wolf_LIBRARIES)
 
   # Reset the CMake module path to its state when this script was called.
@@ -62,4 +62,21 @@ if(NOT wolf_FOUND)
   wolf_report_not_found("Something went wrong while setting up wolf.")
 endif(NOT wolf_FOUND)
 # Set the include directories for wolf (itself).
-set(wolf_FOUND TRUE)
\ No newline at end of file
+set(wolf_FOUND TRUE)
+
+# Now we gather all the required dependencies for Wolf
+
+FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
+list(APPEND wolf_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+
+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})
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 6ee86ed12165a0cc0bc78c93822a0b5a259eddb0..be8d28e875390af10cf0c7d8c7eae4e998612b45 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,228 +1,9 @@
-# Dynamically remove objects from list
-# add_executable(test_list_remove test_list_remove.cpp)
-
-# Matrix product test
-#ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
-
-#ADD_EXECUTABLE(test_eigen_template test_eigen_template.cpp)
-
-ADD_EXECUTABLE(test_fcn_ptr test_fcn_ptr.cpp)
-
-ADD_EXECUTABLE(test_kf_callback test_kf_callback.cpp)
-TARGET_LINK_LIBRARIES(test_kf_callback ${PROJECT_NAME})
-
-ADD_EXECUTABLE(test_wolf_logging test_wolf_logging.cpp)
-TARGET_LINK_LIBRARIES(test_wolf_logging ${PROJECT_NAME})
-
-IF(Ceres_FOUND)
-    # test_processor_odom_3D
-    ADD_EXECUTABLE(test_processor_odom_3D test_processor_odom_3D.cpp)
-    TARGET_LINK_LIBRARIES(test_processor_odom_3D ${PROJECT_NAME})
-
-#    ADD_EXECUTABLE(test_motion_2d test_motion_2d.cpp)
-#    TARGET_LINK_LIBRARIES(test_motion_2d ${PROJECT_NAME})
-ENDIF(Ceres_FOUND)
-
-
-
-
-# State blocks with local parametrizations test
-#ADD_EXECUTABLE(test_state_quaternion test_state_quaternion.cpp)
-#TARGET_LINK_LIBRARIES(test_state_quaternion ${PROJECT_NAME})
-
-# Testing Eigen Permutations
-#ADD_EXECUTABLE(test_permutations solver/test_permutations.cpp)
-#TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME})
-
 # Enable Yaml config files
-IF(YAMLCPP_FOUND)
-#    ADD_EXECUTABLE(test_yaml test_yaml.cpp)
-#    TARGET_LINK_LIBRARIES(test_yaml ${PROJECT_NAME})
-
-#    ADD_EXECUTABLE(test_yaml_conversions test_yaml_conversions.cpp)
-#    TARGET_LINK_LIBRARIES(test_yaml_conversions ${PROJECT_NAME})
-
-    # SensorFactory classes test
-#    ADD_EXECUTABLE(test_wolf_factories test_wolf_factories.cpp)
-#    TARGET_LINK_LIBRARIES(test_wolf_factories ${PROJECT_NAME})
-
-    # Map load and save test
-#    ADD_EXECUTABLE(test_map_yaml test_map_yaml.cpp)
-#    TARGET_LINK_LIBRARIES(test_map_yaml ${PROJECT_NAME})
-ENDIF(YAMLCPP_FOUND)
-
-IF(Suitesparse_FOUND)
-IF (0) # These cannot compile on MacOSX -- we'll fix it some day.
-    # Testing a ccolamd test
-    ADD_EXECUTABLE(test_ccolamd solver/test_ccolamd.cpp)
-    TARGET_LINK_LIBRARIES(test_ccolamd ${PROJECT_NAME})
-
-    # Testing a blocks ccolamd test
-    ADD_EXECUTABLE(test_ccolamd_blocks solver/test_ccolamd_blocks.cpp)
-    TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME})
-
-    # Testing an incremental blocks ccolamd test
-    ADD_EXECUTABLE(test_incremental_ccolamd_blocks solver/test_incremental_ccolamd_blocks.cpp)
-    TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME})
-
-    # Testing an incremental QR with block ccolamd test
-    ADD_EXECUTABLE(test_iQR solver/test_iQR.cpp)
-    TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME})
-
-    # Testing QR solver test tending to wolf
-    ADD_EXECUTABLE(test_iQR_wolf solver/test_iQR_wolf.cpp)
-    TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME})
-
-    # Testing SPQR simple test
-    #ADD_EXECUTABLE(test_SPQR solver/test_SPQR.cpp)
-    #TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME})
-ENDIF(0)
-
-ENDIF(Suitesparse_FOUND)
-
-# Building and populating the wolf tree
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
 
 # Building and populating the wolf tree from .graph file (TORO)
 #ADD_EXECUTABLE(test_wolf_imported_graph test_wolf_imported_graph.cpp)
 #TARGET_LINK_LIBRARIES(test_wolf_imported_graph ${PROJECT_NAME})
 
-# Comparing performance of wolf auto_diff and ceres auto_diff
-#ADD_EXECUTABLE(test_wolf_autodiffwrapper test_wolf_autodiffwrapper.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_autodiffwrapper ${PROJECT_NAME})
-
-# Prunning wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_prunning test_wolf_prunning.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_prunning ${PROJECT_NAME})
-
-# Sparsification from wolf tree from .graph file (TORO)
-ADD_EXECUTABLE(test_sparsification test_sparsification.cpp)
-TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
-
 # Comparing analytic and autodiff odometry factors
-#ADD_EXECUTABLE(test_analytic_odom_factor test_analytic_odom_factor.cpp)
-#TARGET_LINK_LIBRARIES(test_analytic_odom_factor ${PROJECT_NAME})
-
-# Vision
-IF(vision_utils_FOUND)
-
-    IF(Ceres_FOUND)
-        # Testing many things for the 3D image odometry
-        ADD_EXECUTABLE(test_image test_image.cpp)
-        TARGET_LINK_LIBRARIES(test_image ${PROJECT_NAME})
-
-        # Processor Image Landmark test
-        ADD_EXECUTABLE(test_processor_tracker_landmark_image test_processor_tracker_landmark_image.cpp)
-        TARGET_LINK_LIBRARIES(test_processor_tracker_landmark_image ${PROJECT_NAME})
-
-	    # Simple AHP test
-	    ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp)
-	    TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME})
-
-	    IF (APRILTAG_LIBRARY)
-    		ADD_EXECUTABLE(test_apriltag test_apriltag.cpp)
-    		TARGET_LINK_LIBRARIES(test_apriltag ${PROJECT_NAME})
-    	ENDIF(APRILTAG_LIBRARY)
-
-    ENDIF(Ceres_FOUND)
-
-    # Testing OpenCV functions for projection of points
-    ADD_EXECUTABLE(test_projection_points test_projection_points.cpp)
-    TARGET_LINK_LIBRARIES(test_projection_points ${PROJECT_NAME})
-
-    # Factor test
-    ADD_EXECUTABLE(test_factor_AHP test_factor_AHP.cpp)
-    TARGET_LINK_LIBRARIES(test_factor_AHP ${PROJECT_NAME})
-
-    # ORB tracker test
-    ADD_EXECUTABLE(test_tracker_ORB test_tracker_ORB.cpp)
-    TARGET_LINK_LIBRARIES(test_tracker_ORB ${PROJECT_NAME})
-
-ENDIF(vision_utils_FOUND)
-
-# Processor Tracker Feature test
-ADD_EXECUTABLE(test_processor_tracker_feature test_processor_tracker_feature.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_feature ${PROJECT_NAME})
-
-# Processor Tracker Landmark test
-ADD_EXECUTABLE(test_processor_tracker_landmark test_processor_tracker_landmark.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_landmark ${PROJECT_NAME})
-
-# Processor IMU test
-ADD_EXECUTABLE(test_processor_imu test_processor_imu.cpp)
-TARGET_LINK_LIBRARIES(test_processor_imu ${PROJECT_NAME})
-
-# Processor Diff-Drive test
-ADD_EXECUTABLE(test_processor_diff_drive test_diff_drive.cpp)
-TARGET_LINK_LIBRARIES(test_processor_diff_drive ${PROJECT_NAME})
-
-# MPU6050 IMU test
-#ADD_EXECUTABLE(test_mpu test_mpu.cpp)
-#TARGET_LINK_LIBRARIES(test_mpu ${PROJECT_NAME})
-
-# Processor IMU - Jacobian checking test
-#ADD_EXECUTABLE(test_processor_imu_jacobians test_processor_imu_jacobians.cpp)
-#TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME})
-
-# Test IMU using printed Dock
-#ADD_EXECUTABLE(test_imuDock test_imuDock.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock ${PROJECT_NAME})
-
-# Test IMU with auto KF generation (Humanoids 20017)
-#ADD_EXECUTABLE(test_imuDock_autoKFs test_imuDock_autoKFs.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock_autoKFs ${PROJECT_NAME})
-
-# FactorIMU - factors test
-#ADD_EXECUTABLE(test_imu_constrained0 test_imu_constrained0.cpp)
-#TARGET_LINK_LIBRARIES(test_imu_constrained0 ${PROJECT_NAME})
-
-# IMU - Offline plateform test
-#ADD_EXECUTABLE(test_imuPlateform_Offline test_imuPlateform_Offline.cpp)
-#TARGET_LINK_LIBRARIES(test_imuPlateform_Offline ${PROJECT_NAME})
-
-# IMU - factorIMU
-#ADD_EXECUTABLE(test_factor_imu test_factor_imu.cpp)
-#TARGET_LINK_LIBRARIES(test_factor_imu ${PROJECT_NAME})
-
-# IF (laser_scan_utils_FOUND)
-#     ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
-#     TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
-# #ENDIF (laser_scan_utils_FOUND)
-
-# IF(faramotics_FOUND)
-#     IF (laser_scan_utils_FOUND)
-#         ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
-#         TARGET_LINK_LIBRARIES(test_ceres_2_lasers
-#                                 ${pose_state_time_LIBRARIES}
-#                                 ${faramotics_LIBRARIES}
-#                                 ${PROJECT_NAME})
-#         ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
-#         TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
-#                                 ${pose_state_time_LIBRARIES}
-#                                 ${faramotics_LIBRARIES}
-#                                 ${PROJECT_NAME})
-#         ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
-#         TARGET_LINK_LIBRARIES(test_2_lasers_offline
-#                                 ${pose_state_time_LIBRARIES}
-#                                 ${faramotics_LIBRARIES}
-#                                 ${PROJECT_NAME})
-#         ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
-#         TARGET_LINK_LIBRARIES(test_faramotics_simulation
-#                                 ${pose_state_time_LIBRARIES}
-#                                 ${faramotics_LIBRARIES}
-#                                 ${PROJECT_NAME})
-# #        ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
-# #        TARGET_LINK_LIBRARIES(test_autodiff
-# #                                ${pose_state_time_LIBRARIES}
-# #                                ${faramotics_LIBRARIES}
-# #                                ${PROJECT_NAME})
-# #        IF(Suitesparse_FOUND)
-# #            ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
-# #            TARGET_LINK_LIBRARIES(test_iQR_wolf2
-# #                                ${pose_state_time_LIBRARIES}
-# #                                ${faramotics_LIBRARIES}
-# #                                ${PROJECT_NAME})
-# #        ENDIF(Suitesparse_FOUND)
-# #     ENDIF (laser_scan_utils_FOUND)
-# # ENDIF(faramotics_FOUND)
+#ADD_EXECUTABLE(test_analytic_autodiff_factor test_analytic_autodiff_factor.cpp)
+#TARGET_LINK_LIBRARIES(test_analytic_autodiff_factor ${PROJECT_NAME})
\ No newline at end of file
diff --git a/demos/demo_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp
deleted file mode 100644
index 58e91c6d8a63e81ff0337ee1ec4a7b601cdb9de7..0000000000000000000000000000000000000000
--- a/demos/demo_2_lasers_offline.cpp
+++ /dev/null
@@ -1,321 +0,0 @@
-//std includes
-#include "core/sensor/sensor_GPS_fix.h"
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "core/problem/problem.h"
-#include "core/processor/processor_tracker_landmark_corner.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// laserscanutils
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/laser_scan.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-#include "core/capture/capture_pose.h"
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-void extractVector(std::ifstream& text_file, Eigen::VectorXs& vector, wolf::Scalar& ts)
-{
-    std::string line;
-    std::getline(text_file, line);
-    std::stringstream line_stream(line);
-    line_stream >> ts;
-    for (auto i = 0; i < vector.size(); i++)
-        line_stream >> vector(i);
-}
-
-void extractScan(std::ifstream& text_file, std::vector<float>& scan, wolf::Scalar& ts)
-{
-    std::string line;
-    std::getline(text_file, line);
-    std::stringstream line_stream(line);
-    line_stream >> ts;
-    for (unsigned int i = 0; i < scan.size(); i++)
-        line_stream >> scan[i];
-}
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    std::cout << "\n==================================================================\n";
-    std::cout << "========== 2D Robot with offline odometry and 2 LIDARs =============\n";
-
-    // USER INPUT ============================================================================================
-    if (argc != 2 || atoi(argv[1]) < 1 )
-    {
-        std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
-        std::cout << "     - NI is the number of iterations (NI > 0)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    // FILES INPUT ============================================================================================
-    std::ifstream laser_1_file, laser_2_file, odom_file, groundtruth_file;
-    laser_1_file.open("simulated_laser_1.txt", std::ifstream::in);
-    laser_2_file.open("simulated_laser_2.txt", std::ifstream::in);
-    odom_file.open("simulated_odom.txt", std::ifstream::in);
-    groundtruth_file.open("simulated_groundtruth.txt", std::ifstream::in);
-
-    if (!(laser_1_file.is_open() && laser_2_file.is_open() && odom_file.is_open() && groundtruth_file.is_open()))
-    {
-        std::cout << "Error opening simulated data files. Remember to run test_faramotics_simulation before this test!" << std::endl;
-        return -1;
-    }
-    else
-        std::cout << "Simulated data files opened correctly..." << std::endl;
-
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
-    // INITIALIZATION ============================================================================================
-    //init random generators
-    Scalar odom_std_factor = 0.5;
-    std::default_random_engine generator(1);
-    std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-
-    //variables
-    std::string line;
-    Eigen::VectorXs odom_data = Eigen::VectorXs::Zero(2);
-    Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
-    Eigen::VectorXs ground_truth_pose(3); //last true pose
-    Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
-    Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(6);
-    clock_t t1, t2;
-    Scalar timestamp;
-    TimeStamp ts(0);
-
-    // Wolf initialization
-    Eigen::VectorXs odom_pose = Eigen::VectorXs::Zero(3);
-    //Eigen::VectorXs gps_position = Eigen::VectorXs::Zero(2);
-    Eigen::VectorXs laser_1_params(9), laser_2_params(9);
-    Eigen::VectorXs laser_1_pose(4), laser_2_pose(4); //xyz + theta
-    Eigen::VectorXs laser_1_pose2D(3), laser_2_pose2D(3); //xy + theta
-
-    // odometry intrinsics
-    IntrinsicsOdom2D odom_intrinsics;
-    odom_intrinsics.k_disp_to_disp = odom_std_factor;
-    odom_intrinsics.k_rot_to_rot = odom_std_factor;
-
-    // laser 1 extrinsics and intrinsics
-    extractVector(laser_1_file, laser_1_params, timestamp);
-    extractVector(laser_1_file, laser_1_pose, timestamp);
-    laser_1_pose2D.head<2>() = laser_1_pose.head<2>();
-    laser_1_pose2D(2) = laser_1_pose(3);
-    std::vector<float> scan1(laser_1_params(8)); // number of ranges in a scan
-    IntrinsicsLaser2D laser_1_intrinsics;
-    laser_1_intrinsics.scan_params = laserscanutils::LaserScanParams({laser_1_params(0), laser_1_params(1), laser_1_params(2), laser_1_params(3), laser_1_params(4), laser_1_params(5), laser_1_params(6), laser_1_params(7)});
-
-    ProcessorParamsLaser laser_1_processor_params;
-    laser_1_processor_params.line_finder_params_ = laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2});
-    laser_1_processor_params.new_corners_th = 10;
-
-    // laser 2 extrinsics and intrinsics
-    extractVector(laser_2_file, laser_2_params, timestamp);
-    extractVector(laser_2_file, laser_2_pose, timestamp);
-    laser_2_pose2D.head<2>() = laser_2_pose.head<2>();
-    laser_2_pose2D(2) = laser_2_pose(3);
-    std::vector<float> scan2(laser_2_params(8));
-    IntrinsicsLaser2D laser_2_intrinsics;
-    laser_2_intrinsics.scan_params = laserscanutils::LaserScanParams({laser_2_params(0), laser_2_params(1), laser_2_params(2), laser_2_params(3), laser_2_params(4), laser_2_params(5), laser_2_params(6), laser_2_params(7)});
-
-    ProcessorParamsLaser laser_2_processor_params;
-    laser_2_processor_params.line_finder_params_ = laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2});
-    laser_2_processor_params.new_corners_th = 10;
-
-    Problem problem(FRM_PO_2D);
-    SensorOdom2D* odom_sensor = (SensorOdom2D*)problem.installSensor("ODOM 2D", "odometer", odom_pose, &odom_intrinsics);
-    ProcessorParamsOdom2D odom_params;
-    odom_params.cov_det = 1;
-    odom_params.dist_traveled_th_ = 5;
-    odom_params.elapsed_time_th_ = 10;
-    ProcessorOdom2D* odom_processor = (ProcessorOdom2D*)problem.installProcessor("ODOM 2D", "main odometry", odom_sensor, &odom_params);
-    //SensorBasePtr gps_sensor = problem.installSensor("GPS FIX", "GPS fix", gps_position);
-    SensorBasePtr laser_1_sensor = problem.installSensor("LASER 2D", "front laser", laser_1_pose2D, &laser_1_intrinsics);
-    SensorBasePtr laser_2_sensor = problem.installSensor("LASER 2D", "rear laser", laser_2_pose2D, &laser_2_intrinsics);
-    problem.installProcessor("LASER 2D", "front laser processor", laser_1_sensor, &laser_1_processor_params);
-    problem.installProcessor("LASER 2D", "rear laser processor", laser_2_sensor, &laser_2_processor_params);
-
-    std::cout << "Wolf tree setted correctly!" << std::endl;
-
-    CaptureMotion* odom_capture = new CaptureMotion(ts, odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
-
-    // Initial pose
-    ground_truth_pose << 2, 8, 0;
-    ground_truth.head(3) = ground_truth_pose;
-    odom_trajectory.head(3) = ground_truth_pose;
-
-    // Origin Key Frame with covariance
-    problem.setPrior(ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1, ts, 0.1);
-
-    // 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.minimizer_progress_to_stdout = false;
-    //    ceres_options.line_search_direction_type = ceres::LBFGS;
-    //    ceres_options.max_num_iterations = 100;
-    google::InitGoogleLogging(argv[0]);
-
-    CeresManager ceres_manager(&problem, ceres_options);
-    std::ofstream log_file, landmark_file;  //output file
-
-    std::cout << "START TRAJECTORY..." << std::endl;
-    // START TRAJECTORY ============================================================================================
-    for (unsigned int step = 1; step < n_execution; step++)
-    {
-        //get init time
-        t2 = clock();
-
-        // GROUNDTRUTH ---------------------------
-        //std::cout << "GROUND TRUTH..." << std::endl;
-        t1 = clock();
-        extractVector(groundtruth_file, ground_truth_pose, timestamp);
-        ground_truth.segment(step * 3, 3) = ground_truth_pose;
-
-        // timestamp
-        ts = TimeStamp(timestamp);
-
-        // ODOMETRY DATA -------------------------------------
-        //std::cout << "ODOMETRY DATA..." << std::endl;
-        extractVector(odom_file, odom_data, timestamp);
-        // noisy odometry
-        odom_data(0) += distribution_odom(generator) * (odom_data(0) == 0 ? 1e-6 : odom_data(0));
-        odom_data(1) += distribution_odom(generator) * (odom_data(1) == 0 ? 1e-6 : odom_data(1));
-        // process odometry
-        odom_capture->setTimeStamp(TimeStamp(ts));
-        odom_capture->setData(odom_data);
-        odom_processor->process(odom_capture);
-        // odometry integration
-        odom_trajectory.segment(step * 3, 3) = problem.getCurrentState();
-
-        // LIDAR DATA ---------------------------
-        extractScan(laser_1_file, scan1, timestamp);
-        extractScan(laser_2_file, scan2, timestamp);
-        if (step % 3 == 0)
-        {
-            std::cout << "--PROCESS LIDAR 1 DATA..." << std::endl;
-            CaptureLaser2D* new_scan_1 = new CaptureLaser2D(ts, laser_1_sensor, scan1);
-            new_scan_1->process();
-            std::cout << "--PROCESS LIDAR 2 DATA..." << std::endl;
-            CaptureLaser2D* new_scan_2 = new CaptureLaser2D(ts, laser_2_sensor, scan2);
-            new_scan_2->process();
-        }
-        mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // SOLVE OPTIMIZATION ---------------------------
-        std::cout << "SOLVING..." << std::endl;
-        t1 = clock();
-        ceres::Solver::Summary summary = ceres_manager.solve();
-        //std::cout << summary.FullReport() << std::endl;
-        mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // COMPUTE COVARIANCES ---------------------------
-        std::cout << "COMPUTING COVARIANCES..." << std::endl;
-        t1 = clock();
-        ceres_manager.computeCovariances();
-        mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // TIME MANAGEMENT ---------------------------
-        double total_t = ((double) clock() - t2) / CLOCKS_PER_SEC;
-        mean_times(5) += total_t;
-        if (total_t < 0.2)
-            usleep(200000 - 1e6 * total_t);
-
-//		std::cout << "\nTree after step..." << std::endl;
-    }
-
-    // DISPLAY RESULTS ============================================================================================
-    mean_times /= n_execution;
-    std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
-    std::cout << "  data reading:    " << mean_times(0) << std::endl;
-    std::cout << "  wolf managing:      " << mean_times(1) << std::endl;
-    std::cout << "  ceres managing:     " << mean_times(2) << std::endl;
-    std::cout << "  ceres optimization: " << mean_times(3) << std::endl;
-    std::cout << "  ceres covariance:   " << mean_times(4) << std::endl;
-    std::cout << "  loop time:          " << mean_times(5) << std::endl;
-
-    //	std::cout << "\nTree before deleting..." << std::endl;
-
-    // Print Final result in a file -------------------------
-    // Vehicle poses
-    int i = 0;
-    Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
-    for (auto frame : *(problem.getTrajectory()->getFrameList()))
-    {
-        state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
-        i += 3;
-    }
-
-    // Landmarks
-    i = 0;
-    Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
-    for (auto landmark : *(problem.getMap()->getLandmarkList()))
-    {
-        landmarks.segment(i, 2) = landmark->getP()->getVector();
-        i += 2;
-    }
-
-    // Print log files
-    std::string filepath = getenv("HOME") + std::string("/Desktop/log_file_2.txt");
-    log_file.open(filepath, std::ofstream::out); //open log file
-
-    if (log_file.is_open())
-    {
-        log_file << 0 << std::endl;
-        for (unsigned int ii = 0; ii < n_execution; ii++)
-            log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
-        log_file.close(); //close log file
-        std::cout << std::endl << "Result file " << filepath << std::endl;
-    }
-    else
-        std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
-    std::string filepath2 = getenv("HOME") + std::string("/Desktop/landmarks_file_2.txt");
-    landmark_file.open(filepath2, std::ofstream::out); //open log file
-
-    if (landmark_file.is_open())
-    {
-        for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
-            landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
-        landmark_file.close(); //close log file
-        std::cout << std::endl << "Landmark file " << filepath << std::endl;
-    }
-    else
-        std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
-    std::cout << "Press any key for ending... " << std::endl << std::endl;
-    std::getchar();
-
-    std::cout << "Problem:" << std::endl;
-    std::cout << "Frames: " << problem.getTrajectory()->getFrameList().size() << std::endl;
-    std::cout << "Landmarks: " << problem.getMap()->getLandmarkList()->size() << std::endl;
-    std::cout << "Sensors: " << problem.getHardware()->getSensorList()->size() << std::endl;
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    //exit
-    return 0;
-}
diff --git a/demos/demo_analytic_odom_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
similarity index 100%
rename from demos/demo_analytic_odom_factor.cpp
rename to demos/demo_analytic_autodiff_factor.cpp
diff --git a/demos/demo_autodiff.cpp b/demos/demo_autodiff.cpp
deleted file mode 100644
index 6fa1e01edddf6861f52c6c50222be29521ce68a2..0000000000000000000000000000000000000000
--- a/demos/demo_autodiff.cpp
+++ /dev/null
@@ -1,422 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-//laser_scan_utils
-#include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
-
-namespace wolf {
-//function travel around
-void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
-{
-    if (ii <= 120){         displacement_ = 0.1;    rotation_ = 0; }
-    else if (ii <= 170) {   displacement_ = 0.2;    rotation_ = 1.8 * M_PI / 180; }
-    else if (ii <= 220) {   displacement_ = 0;      rotation_ =-1.8 * M_PI / 180; }
-    else if (ii <= 310) {   displacement_ = 0.1;    rotation_ = 0; }
-    else if (ii <= 487) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-    else if (ii <= 600) {   displacement_ = 0.2;    rotation_ = 0; }
-    else if (ii <= 700) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-    else if (ii <= 780) {   displacement_ = 0;      rotation_ =-1.0 * M_PI / 180; }
-    else {                  displacement_ = 0.3;    rotation_ = 0; }
-
-    pose.moveForward(displacement_);
-    pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
-}
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
-
-    // USER INPUT ============================================================================================
-    if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1 )
-    {
-        std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
-        std::cout << "     - NI is the number of iterations (NI > 0)" << std::endl;
-        std::cout << "     - WS is the window size (WS > 0)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-    unsigned int window_size = (unsigned int) atoi(argv[2]);
-
-    // INITIALIZATION ============================================================================================
-    //init random generators
-    Scalar odom_std_factor = 0.5;
-    Scalar gps_std = 1;
-    std::default_random_engine generator(1);
-    std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-    std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
-
-    //init google log
-    //google::InitGoogleLogging(argv[0]);
-
-    // Faramotics stuff
-    Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
-    vector < Cpose3d > devicePoses;
-    vector<float> scan1, scan2;
-    string modelFileName;
-
-    //model and initial view point
-    modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
-    devicePose.setPose(2, 8, 0.2, 0, 0, 0);
-    viewPoint.setPose(devicePose);
-    viewPoint.moveForward(10);
-    viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
-    viewPoint.moveForward(-15);
-    //glut initialization
-    faramotics::initGLUT(argc, argv);
-
-    //create a viewer for the 3D model and scan points
-    CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
-    myRender->loadAssimpModel(modelFileName, true); //with wireframe
-    //create scanner and load 3D model
-    CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);	//HOKUYO_UTM30LX_180DEG or LEUZE_RS4
-    myScanner->loadAssimpModel(modelFileName);
-
-    //variables
-    Eigen::Vector3s odom_reading;
-    Eigen::Vector2s gps_fix_reading;
-    Eigen::VectorXs pose_odom(3); //current odometry integred pose
-    Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
-    Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
-    Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
-    clock_t t1, t2;
-
-    // Wolf manager initialization
-    Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
-    Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
-    Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
-    laser_1_pose << 1.2, 0, 0, 0; //laser 1
-    laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-    SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
-    SensorGPSFix gps_sensor(std::make_shared<StateBlock>(gps_pose.head(2)), std::make_shared<StateBlock>(gps_pose.tail(1)), gps_std);
-    SensorLaser2D laser_1_sensor(std::make_shared<StateBlock>(laser_1_pose.head(2)), std::make_shared<StateBlock>(laser_1_pose.tail(1)));
-    SensorLaser2D laser_2_sensor(std::make_shared<StateBlock>(laser_2_pose.head(2)), std::make_shared<StateBlock>(laser_2_pose.tail(1)));
-    laser_1_sensor.addProcessor(new ProcessorLaser2D());
-    laser_2_sensor.addProcessor(new ProcessorLaser2D());
-
-    // Initial pose
-    pose_odom << 2, 8, 0;
-    ground_truth.head(3) = pose_odom;
-    odom_trajectory.head(3) = pose_odom;
-
-    WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
-    WolfManager* wolf_manager_wolf = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
-    
-    // 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.minimizer_progress_to_stdout = false;
-    //    ceres_options.line_search_direction_type = ceres::LBFGS;
-    //    ceres_options.max_num_iterations = 100;
-    CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblem(), false);
-    CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblem(), true);
-    std::ofstream log_file, landmark_file;  //output file
-
-    //std::cout << "START TRAJECTORY..." << std::endl;
-    // START TRAJECTORY ============================================================================================
-    for (unsigned int step = 1; step < n_execution; step++)
-    {
-        //get init time
-        t2 = clock();
-
-        // ROBOT MOVEMENT ---------------------------
-        //std::cout << "ROBOT MOVEMENT..." << std::endl;
-        // moves the device position
-        t1 = clock();
-        motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
-        odom_reading(1) = 0;
-        devicePoses.push_back(devicePose);
-
-        // SENSOR DATA ---------------------------
-        //std::cout << "SENSOR DATA..." << std::endl;
-        // store groundtruth
-        ground_truth.segment(step * 3, 3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-
-        // compute odometry
-        odom_reading(0) += distribution_odom(generator) * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
-        odom_reading(1) += distribution_odom(generator) * 1e-6;
-        odom_reading(2) += distribution_odom(generator) * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
-
-        // odometry integration
-        pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
-        pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)) + odom_reading(1) * cos(pose_odom(2));
-        pose_odom(2) = pose_odom(2) + odom_reading(1);
-        odom_trajectory.segment(step * 3, 3) = pose_odom;
-
-        // compute GPS
-        gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
-        gps_fix_reading(0) += distribution_gps(generator);
-        gps_fix_reading(1) += distribution_gps(generator);
-
-        //compute scans
-        scan1.clear();
-        scan2.clear();
-        // scan 1
-        laser1Pose.setPose(devicePose);
-        laser1Pose.moveForward(laser_1_pose(0));
-        myScanner->computeScan(laser1Pose, scan1);
-        // scan 2
-        laser2Pose.setPose(devicePose);
-        laser2Pose.moveForward(laser_2_pose(0));
-        laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-        myScanner->computeScan(laser2Pose, scan2);
-
-        mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // ADD CAPTURES ---------------------------
-        std::cout << "ADD CAPTURES..." << std::endl;
-        t1 = clock();
-        // adding new sensor captures
-        wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading));		//, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
-        wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
-        wolf_manager_ceres->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
-        wolf_manager_ceres->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
-        wolf_manager_wolf->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading));       //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
-        wolf_manager_wolf->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
-        wolf_manager_wolf->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
-        wolf_manager_wolf->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
-
-        // updating problem
-        wolf_manager_ceres->update();
-        wolf_manager_wolf->update();
-        mean_times(1) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // UPDATING CERES ---------------------------
-        std::cout << "UPDATING CERES..." << std::endl;
-        t1 = clock();
-        // update state units and factors in ceres
-        ceres_manager_ceres->update();
-        ceres_manager_wolf->update();
-        mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // SOLVE OPTIMIZATION ---------------------------
-        std::cout << "SOLVING..." << std::endl;
-        t1 = clock();
-        ceres::Solver::Summary summary_ceres = ceres_manager_ceres->solve(ceres_options);
-        ceres::Solver::Summary summary_wolf = ceres_manager_wolf->solve(ceres_options);
-        std::cout << "CERES AUTO DIFF" << std::endl;
-        std::cout << "Jacobian evaluation: " << summary_ceres.jacobian_evaluation_time_in_seconds << std::endl;
-        std::cout << "Total time: " << summary_ceres.total_time_in_seconds << std::endl;
-        std::cout << "WOLF AUTO DIFF" << std::endl;
-        std::cout << "Jacobian evaluation: " << summary_wolf.jacobian_evaluation_time_in_seconds << std::endl;
-        std::cout << "Total time: " << summary_wolf.total_time_in_seconds << std::endl;
-        mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        std::cout << "CERES AUTO DIFF solution:" << std::endl;
-        std::cout << wolf_manager_ceres->getVehiclePose().transpose() << std::endl;
-        std::cout << "WOLF AUTO DIFF solution:" << std::endl;
-        std::cout << wolf_manager_wolf->getVehiclePose().transpose() << std::endl;
-
-        // COMPUTE COVARIANCES ---------------------------
-        std::cout << "COMPUTING COVARIANCES..." << std::endl;
-        t1 = clock();
-        ceres_manager_ceres->computeCovariances(ALL_MARGINALS);
-        ceres_manager_wolf->computeCovariances(ALL_MARGINALS);
-        Eigen::MatrixXs marginal_ceres(3,3), marginal_wolf(3,3);
-        wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
-                                                                wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
-                                                                marginal_ceres, 0, 0);
-        wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
-                                                                wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
-                                                                marginal_ceres, 0, 2);
-        wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
-                                                                wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
-                                                                marginal_ceres, 2, 2);
-        wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
-                                                               wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
-                                                               marginal_wolf, 0, 0);
-        wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
-                                                               wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
-                                                               marginal_wolf, 0, 2);
-        wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
-                                                               wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
-                                                               marginal_wolf, 2, 2);
-        std::cout << "CERES AUTO DIFF covariance:" << std::endl;
-        std::cout << marginal_ceres << std::endl;
-        std::cout << "WOLF AUTO DIFF covariance:" << std::endl;
-        std::cout << marginal_wolf << std::endl;
-        mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // DRAWING STUFF ---------------------------
-        t1 = clock();
-        // draw detected corners
-//        std::list < laserscanutils::Corner > corner_list;
-//        std::vector<double> corner_vector;
-//        CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
-//        last_scan.extractCorners(corner_list);
-//        for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
-//        {
-//            corner_vector.push_back(corner_it->pt_(0));
-//            corner_vector.push_back(corner_it->pt_(1));
-//        }
-//        myRender->drawCorners(laser1Pose, corner_vector);
-
-//        // draw landmarks
-//        std::vector<double> landmark_vector;
-//        for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
-//        {
-//            Scalar* position_ptr = (*landmark_it)->getP()->get();
-//            landmark_vector.push_back(*position_ptr); //x
-//            landmark_vector.push_back(*(position_ptr + 1)); //y
-//            landmark_vector.push_back(0.2); //z
-//        }
-//        myRender->drawLandmarks(landmark_vector);
-//
-//        // draw localization and sensors
-//        estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0), wolf_manager->getVehiclePose()(1), 0.2, wolf_manager->getVehiclePose()(2), 0, 0);
-//        estimated_laser_1_pose.setPose(estimated_vehicle_pose);
-//        estimated_laser_1_pose.moveForward(laser_1_pose(0));
-//        estimated_laser_2_pose.setPose(estimated_vehicle_pose);
-//        estimated_laser_2_pose.moveForward(laser_2_pose(0));
-//        estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
-//        myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-//
-//        //Set view point and render the scene
-//        //locate visualization view point, somewhere behind the device
-////		viewPoint.setPose(devicePose);
-////		viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-////		viewPoint.moveForward(-5);
-//        myRender->setViewPoint(viewPoint);
-//        myRender->drawPoseAxis(devicePose);
-//        myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
-//        myRender->render();
-//        mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // TIME MANAGEMENT ---------------------------
-        double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
-        mean_times(6) += dt;
-        if (dt < 0.1)
-            usleep(100000 - 1e6 * dt);
-
-//		std::cout << "\nTree after step..." << std::endl;
-    }
-
-    // DISPLAY RESULTS ============================================================================================
-    mean_times /= n_execution;
-    std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
-    std::cout << "  data generation:    " << mean_times(0) << std::endl;
-    std::cout << "  wolf managing:      " << mean_times(1) << std::endl;
-    std::cout << "  ceres managing:     " << mean_times(2) << std::endl;
-    std::cout << "  ceres optimization: " << mean_times(3) << std::endl;
-    std::cout << "  ceres covariance:   " << mean_times(4) << std::endl;
-    std::cout << "  results drawing:    " << mean_times(5) << std::endl;
-    std::cout << "  loop time:          " << mean_times(6) << std::endl;
-
-//	std::cout << "\nTree before deleting..." << std::endl;
-
-//    // Draw Final result -------------------------
-//    std::vector<double> landmark_vector;
-//    for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
-//    {
-//        Scalar* position_ptr = (*landmark_it)->getP()->get();
-//        landmark_vector.push_back(*position_ptr); //x
-//        landmark_vector.push_back(*(position_ptr + 1)); //y
-//        landmark_vector.push_back(0.2); //z
-//    }
-//    myRender->drawLandmarks(landmark_vector);
-////	viewPoint.setPose(devicePoses.front());
-////	viewPoint.moveForward(10);
-////	viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-////	viewPoint.moveForward(-10);
-//    myRender->setViewPoint(viewPoint);
-//    myRender->render();
-
-    // Print Final result in a file -------------------------
-    // Vehicle poses
-//    int i = 0;
-//    Eigen::VectorXs state_poses(n_execution * 3);
-//    for (auto frame_it = wolf_manager->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
-//    {
-//        state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
-//        i += 3;
-//    }
-//
-//    // Landmarks
-//    i = 0;
-//    Eigen::VectorXs landmarks(wolf_manager->getProblem()->getMap()->getLandmarkList().size() * 2);
-//    for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
-//    {
-//        Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
-//        landmarks.segment(i, 2) = landmark;
-//        i += 2;
-//    }
-//
-//    // Print log files
-//    std::string filepath = getenv("HOME") + std::string("/Desktop/log_file_2.txt");
-//    log_file.open(filepath, std::ofstream::out); //open log file
-//
-//    if (log_file.is_open())
-//    {
-//        log_file << 0 << std::endl;
-//        for (unsigned int ii = 0; ii < n_execution; ii++)
-//            log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
-//        log_file.close(); //close log file
-//        std::cout << std::endl << "Result file " << filepath << std::endl;
-//    }
-//    else
-//        std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-//
-//    std::string filepath2 = getenv("HOME") + std::string("/Desktop/landmarks_file_2.txt");
-//    landmark_file.open(filepath2, std::ofstream::out); //open log file
-//
-//    if (landmark_file.is_open())
-//    {
-//        for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
-//            landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
-//        landmark_file.close(); //close log file
-//        std::cout << std::endl << "Landmark file " << filepath << std::endl;
-//    }
-//    else
-//        std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-//
-//    std::cout << "Press any key for ending... " << std::endl << std::endl;
-//    std::getchar();
-
-    delete myRender;
-    delete myScanner;
-    delete wolf_manager_ceres;
-    delete wolf_manager_wolf;
-    std::cout << "wolf deleted" << std::endl;
-    delete ceres_manager_ceres;
-    delete ceres_manager_wolf;
-    std::cout << "ceres_manager deleted" << std::endl;
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    //exit
-    return 0;
-}
diff --git a/demos/demo_capture_laser_2D.cpp b/demos/demo_capture_laser_2D.cpp
deleted file mode 100644
index cd5e40239fd42ca972a0e96405dc609ab707e9ca..0000000000000000000000000000000000000000
--- a/demos/demo_capture_laser_2D.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-
-//std
-#include <random>
-
-//wolf
-#include "core/capture/capture_laser_2D.h"
-
-// Eigen in std vector
-#include <Eigen/StdVector>
-
-//main
-int main(int argc, char *argv[])
-{
-    using namespace wolf;
-
-    std::cout << std::endl << "CaptureLaser2D class test" << std::endl;
-    std::cout << "========================================================" << std::endl;
-    
-    //scan ranges
-    Eigen::VectorXs ranges(720);
-    ranges << 	2.78886,2.78289,2.7832,2.78367,2.7843,2.78508,2.78603,2.78713,2.78839,2.78981,
-		2.79139,2.78669,2.78855,2.79057,2.79274,2.79507,2.79098,2.79359,2.79635,2.79927,
-		2.79566,2.79886,2.80221,2.79895,2.80258,2.80638,2.80345,2.80753,2.81176,2.80917,
-		2.81368,2.81132,2.81611,2.81396,2.81903,2.73664,2.72743,2.71782,2.70883,2.69945,
-		2.69067,2.6815,2.67294,2.66398,2.65562,2.64736,2.6387,2.63064,2.62218,2.61431,
-		2.60605,2.59837,2.59078,2.5828,2.57539,2.56759,2.56036,2.55321,2.54568,2.53871,
-		2.53182,2.52455,2.51782,2.51118,2.50415,2.49767,2.49127,2.48495,2.47824,2.47207,
-		2.46597,2.4595,2.45355,2.44768,2.44188,2.4357,2.43005,2.42446,2.41894,2.4135,
-		2.40768,2.40236,2.39712,2.39195,2.3864,2.38135,2.37637,2.37146,2.36661,2.36182,
-		2.35666,2.352,2.3474,2.34286,2.34186,2.36289,2.41051,2.43311,2.45628,2.48003,
-		2.50439,2.52937,2.555,2.61224,2.63993,2.66837,2.69757,2.72758,2.75841,2.79011,
-		2.82269,2.85621,2.8907,2.96659,3.0042,3.04295,3.08289,3.12406,3.16652,3.21033,
-		3.22693,3.23444,3.24207,3.24982,3.2577,3.26571,3.2855,3.29383,3.30229,3.31088,
-		3.3196,3.32846,3.33745,3.34658,3.35584,3.36524,3.37478,3.38445,3.39427,3.40423,
-		3.41434,3.42459,3.43499,3.44553,3.45623,3.46707,3.47807,3.48923,3.50053,3.512,
-		3.52362,3.53541,3.54736,3.55947,3.57175,3.5842,3.59681,3.6096,3.62256,3.63569,
-		3.64901,3.6625,3.67617,3.69003,3.70407,3.7183,3.73272,3.74733,3.76214,3.7931,
-		3.80843,3.82397,3.83972,3.85567,3.87183,3.88822,3.90481,3.92163,3.93867,3.95593,
-		3.97343,3.97347,3.99127,4.00931,4.02758,4.0461,4.06486,4.08387,4.10314,4.12266,
-		4.14244,4.16248,4.20237,4.22313,4.24418,4.26551,4.28712,4.30903,4.33122,4.35372,
-		4.37652,4.39963,4.42304,4.44678,4.47084,4.49523,4.51994,4.545,4.57041,4.59615,
-		4.62225,4.64871,4.67554,4.70274,4.73032,4.75828,4.78663,4.81538,4.84452,4.8741,
-		4.90409,4.9345,4.96534,4.99662,5.02836,5.06053,5.09317,5.12628,5.15987,5.19395,
-		5.22853,5.2636,5.2992,5.33532,5.37197,5.44106,5.47923,5.51796,5.55729,5.59721,
-		5.63773,5.67888,5.72066,5.76307,5.80615,5.8499,5.89432,5.93945,6.02374,6.07085,
-		6.11872,6.16734,6.21676,6.26698,6.318,6.36987,6.42258,6.47618,6.5758,6.6319,
-		6.68894,6.74694,6.80593,6.86593,6.92698,7.04022,7.10425,7.1694,7.23572,7.30322,
-		7.37192,7.49928,7.57149,7.64505,7.58372,7.51951,7.45681,7.32129,7.32938,7.34276,
-		7.35632,7.36877,7.38272,7.39687,7.41124,7.4258,7.43923,7.4542,7.46937,7.48477,
-		7.49904,7.51485,7.53087,7.54579,7.56225,7.57894,7.59587,7.61164,7.62902,8.37389,
-		8.39194,8.41173,8.43177,8.45059,8.47118,8.49202,8.51164,8.53305,8.55319,8.57516,
-		8.59739,8.61839,8.64122,8.6628,8.6862,8.70837,8.73237,8.7567,8.77979,8.80472,
-		8.82843,8.85402,8.87835,8.9046,8.9296,8.9565,8.98218,9.0082,9.03616,9.06287,
-		9.09152,9.11895,9.14834,9.17652,9.20503,9.23557,9.26487,9.29454,9.32626,9.35674,
-		9.38762,9.42055,9.45226,9.4844,9.51695,9.55162,9.58503,9.61893,9.65324,4.38929,
-		4.38536,4.36058,4.3365,4.3131,4.29036,4.26827,4.24682,4.22598,4.20576,4.18612,
-		4.1944,4.17582,4.15708,4.13859,4.12032,4.10229,4.08449,4.06691,4.04955,4.03241,
-		4.01549,3.99916,3.98265,3.96634,3.95024,3.93434,3.91901,3.90349,3.88817,3.87304,
-		3.85845,3.84368,3.82909,3.81504,3.80081,3.78674,3.7732,3.75948,3.74591,3.73287,
-		3.71963,3.7069,3.69398,3.68156,3.66894,3.65647,3.6445,3.63233,3.62065,3.60876,
-		3.59736,3.58576,3.58265,3.61553,3.62696,3.63867,3.67347,3.68596,3.72229,3.7356,
-		3.77355,3.78772,3.80219,3.84244,3.85785,3.89993,3.9163,3.93303,3.97774,3.99551,
-		4.01367,4.06121,4.0805,4.10019,4.15081,4.17174,4.19309,13.8062,13.7714,13.7384,
-		13.7075,13.8936,13.9735,14.0549,14.1382,14.3407,15.8017,15.9009,16.002,16.1054,
-		16.3519,16.462,16.5744,16.6893,16.9594,17.0819,17.2072,17.3352,17.4661,17.6,
-		8.14878,8.1334,8.11823,8.10324,8.08848,8.07391,8.0588,8.04465,8.03069,8.01693,
-		8.00338,7.99,7.97684,7.96312,7.95032,7.93773,7.92533,7.91309,7.90106,7.88922,7.87755,
-		7.86606,7.85476,7.84289,7.83195,7.82116,7.81058,7.80017,7.78993,7.77984,7.76995,
-		7.76021,7.75066,7.74128,7.73204,7.76034,7.99805,8.11853,8.24311,8.37202,12.3718,
-		12.3587,12.346,12.3336,12.3213,12.3094,12.2976,12.2862,12.275,12.264,12.2534,12.2429,
-		12.2327,12.2228,12.213,12.2036,12.1944,12.1854,12.1766,12.3577,12.667,16.7608,16.7501,
-		16.7398,16.7297,16.7201,16.7106,16.7015,16.6929,16.6844,16.9488,20.069,20.0619,20.0552,
-		20.0489,20.043,20.0374,20.0323,20.0275,20.0231,20.0191,20.0155,20.0122,20.0094,20.0069,
-		20.0048,20.0031,20.0018,20.0008,20.0002,20.0001,20.0002,20.0008,20.0018,20.0031,20.0048,
-		20.0069,20.0094,20.0122,20.0155,20.0191,20.0231,20.0275,20.0323,20.0374,20.043,20.0489,
-		20.0552,20.0619,20.069,20.0764,20.0843,20.0926,20.1012,20.1102,20.1196,20.1294,20.1397,
-		20.1502,20.1612,20.1726,20.1844,20.1966,20.2092,20.2222,20.2356,20.2494,20.2636,20.2782,
-		20.2932,20.3086,20.3244,20.3407,20.3573,20.3744,20.3919,20.4098,20.4281,20.4469,20.466,
-		20.4856,20.5057,20.5261,20.547,20.5684,20.5901,20.6123,20.635,20.6581,20.6816,20.7056,
-		20.73,20.7549,20.7802,20.806,20.8323,20.859,20.8862,20.9139,20.942,20.9706,20.9997,
-		21.0293,21.0594,21.0899,21.1209,21.1525,21.1845,21.217,21.2501,21.2836,21.3177,21.3522,
-		21.3873,21.423,21.4591,21.4958,21.533,21.5707,21.6091,21.6479,21.6873,21.7273,21.7678,
-		21.8089,21.8505,21.8928,21.9356,21.979,22.023,22.0676,22.1128,22.1586,22.2051,22.2521,
-		22.2998,22.3481,22.397,22.4466,22.4968,22.5477,22.5992,22.6515,22.7043,22.7579,22.8122,
-		22.8671,22.9228,22.9792,23.0363,23.0941,23.1526,23.2119,23.2719,23.3327,23.3943,23.4566,
-		23.5197,23.5836,23.6483,23.7138,23.7802,23.8473,23.9153,23.9842,24.0539,24.1244,24.1959,
-		24.2682,24.3414,24.4156,24.4906,24.5666,24.6435,24.7214,24.8003,24.8801,24.9609,25.0428,
-		25.1256,25.2095,25.2944,25.3804,25.4675,25.5556,25.6449,25.7353,25.8268,25.9194,26.0132,
-		26.1082,26.2044,26.3018,26.4004,26.5003,26.6015,26.7039,26.8077,26.9127,27.0191,27.1269,
-		27.2361,27.3466,27.4586,27.572,27.6869,27.8033,27.9213,28.0407,28.1617;
-    
-    //variable declarations and inits
-    Eigen::VectorXs device_pose(6);
-    device_pose << 0,0,0,0,0,0; //origin, no rotation
-    TimeStamp time_stamp;
-    time_stamp.setToNow();
-    std::list<Eigen::Vector4s, Eigen::aligned_allocator<Eigen::Vector4s> > corner_list;
-    
-    //Create Device objects 
-    //SensorLaser2D device(device_pose, ranges.size(), M_PI, 0.2, 30.0, 0.01);
-    SensorLaser2D device(device_pose, -M_PI/2, M_PI/2, M_PI/ranges.size(), 0.2, 30.0, 0.01);
-    device.printSensorParameters();
-    
-    //init a noise generator
-    std::default_random_engine generator(1);
-    std::normal_distribution<Scalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise
-    
-    //Create a Capture object
-    CaptureLaser2D capture(time_stamp, &device, ranges);
-
-    //add noise to measurements
-    //TODO
-    
-    //do things with the measurements
-	clock_t t1, t2;
-	t1=clock();
-    capture.extractCorners(corner_list);
-    t2=clock();
-	std::cout << "seconds = " << ((double)t2-t1)/CLOCKS_PER_SEC << std::endl;
-    capture.createFeatures(corner_list);
-    capture.printSelf();
-
-    //print corners
-    std::cout << "CORNER LIST" << std::endl;            
-    for (auto corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it ++ )
-    {
-        std::cout << corner_it->x() << " , " << corner_it->y() << std::endl;
-    }
-    
-    std::cout << "========================================================" << std::endl;            
-    std::cout << std::endl << "End CaptureLaser2D class test" << std::endl;
-    return 0;
-}
-
diff --git a/demos/demo_ceres_2_lasers.cpp b/demos/demo_ceres_2_lasers.cpp
deleted file mode 100644
index dce55b16f3fd730ddaf8b0832736bb79bd30bccb..0000000000000000000000000000000000000000
--- a/demos/demo_ceres_2_lasers.cpp
+++ /dev/null
@@ -1,420 +0,0 @@
-//std includes
-#include "core/sensor/sensor_GPS_fix.h"
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "core/problem/problem.h"
-#include "core/processor/processor_tracker_landmark_corner.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// laserscanutils
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/laser_scan.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-#include "core/capture/capture_pose.h"
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-namespace wolf {
-class FaramoticsRobot
-{
-    public:
-
-        Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
-        vector < Cpose3d > devicePoses;
-        vector<float> scan1, scan2;
-        string modelFileName;
-        CrangeScan2D* myScanner;
-        CdynamicSceneRender* myRender;
-        Eigen::Vector3s ground_truth_pose_;
-        Eigen::Vector4s laser_1_pose_, laser_2_pose_;
-
-        FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
-            modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
-            laser_1_pose_(_laser_1_pose),
-            laser_2_pose_(_laser_2_pose)
-        {
-            devicePose.setPose(2, 8, 0.2, 0, 0, 0);
-            viewPoint.setPose(devicePose);
-            viewPoint.moveForward(10);
-            viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
-            viewPoint.moveForward(-15);
-            //glut initialization
-            faramotics::initGLUT(argc, argv);
-            //create a viewer for the 3D model and scan points
-            myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
-            myRender->loadAssimpModel(modelFileName, true); //with wireframe
-            //create scanner and load 3D model
-            myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);  //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
-            myScanner->loadAssimpModel(modelFileName);
-        }
-
-        //function travel around
-        Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
-        {
-            if (ii <= 120){         displacement_ = 0.1;    rotation_ = 0; }
-            else if (ii <= 170) {   displacement_ = 0.2;    rotation_ = 1.8 * M_PI / 180; }
-            else if (ii <= 220) {   displacement_ = 0;      rotation_ =-1.8 * M_PI / 180; }
-            else if (ii <= 310) {   displacement_ = 0.1;    rotation_ = 0; }
-            else if (ii <= 487) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-            else if (ii <= 600) {   displacement_ = 0.2;    rotation_ = 0; }
-            else if (ii <= 700) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-            else if (ii <= 780) {   displacement_ = 0;      rotation_ =-1.0 * M_PI / 180; }
-            else {                  displacement_ = 0.3;    rotation_ = 0; }
-
-            devicePose.moveForward(displacement_);
-            devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
-
-            // laser 1
-            laser1Pose.setPose(devicePose);
-            laser1Pose.moveForward(laser_1_pose_(0));
-            // laser 2
-            laser2Pose.setPose(devicePose);
-            laser2Pose.moveForward(laser_2_pose_(0));
-            laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-
-            devicePoses.push_back(devicePose);
-
-            ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-            return ground_truth_pose_;
-        }
-
-        ~FaramoticsRobot()
-        {
-            std::cout << "deleting render and scanner.." << std::endl;
-            delete myRender;
-            delete myScanner;
-            std::cout << "deleted!" << std::endl;
-        }
-
-        //compute scans
-        vector<float> computeScan(const int scan_id)
-        {
-            if (scan_id == 1)
-            {
-                scan1.clear();
-                myScanner->computeScan(laser1Pose, scan1);
-                return scan1;
-            }
-            else
-            {
-                scan2.clear();
-                myScanner->computeScan(laser2Pose, scan2);
-                return scan2;
-            }
-        }
-
-        void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
-        {
-            // detected corners
-            //std::cout << "   drawCorners: " << feature_list.size() << std::endl;
-            std::vector<double> corner_vector;
-            corner_vector.reserve(2*feature_list.size());
-            for (auto corner : feature_list)
-            {
-                //std::cout << "       corner " << corner->id() << std::endl;
-                corner_vector.push_back(corner->getMeasurement(0));
-                corner_vector.push_back(corner->getMeasurement(1));
-            }
-            myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
-
-            // landmarks
-            //std::cout << "   drawLandmarks: " << landmark_list.size() << std::endl;
-            std::vector<double> landmark_vector;
-            landmark_vector.reserve(3*landmark_list.size());
-            for (auto landmark : landmark_list)
-            {
-                Scalar* position_ptr = landmark->getP()->get();
-                landmark_vector.push_back(*position_ptr); //x
-                landmark_vector.push_back(*(position_ptr + 1)); //y
-                landmark_vector.push_back(0.2); //z
-            }
-            myRender->drawLandmarks(landmark_vector);
-
-            // draw localization and sensors
-            estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
-            estimated_laser_1_pose.setPose(estimated_vehicle_pose);
-            estimated_laser_1_pose.moveForward(laser_1_pose_(0));
-            estimated_laser_2_pose.setPose(estimated_vehicle_pose);
-            estimated_laser_2_pose.moveForward(laser_2_pose_(0));
-            estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
-            myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
-            //Set view point and render the scene
-            //locate visualization view point, somewhere behind the device
-    //      viewPoint.setPose(devicePose);
-    //      viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-    //      viewPoint.moveForward(-5);
-            myRender->setViewPoint(viewPoint);
-            myRender->drawPoseAxis(devicePose);
-            myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
-            myRender->render();
-        }
-};
-}
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    std::cout << "\n============================================================\n";
-    std::cout << "========== 2D Robot with odometry and 2 LIDARs =============\n";
-
-    // USER INPUT ============================================================================================
-    if (argc != 2 || atoi(argv[1]) < 1 )
-    {
-        std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
-        std::cout << "     - NI is the number of iterations (NI > 0)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
-    // INITIALIZATION ============================================================================================
-    //init random generators
-    Scalar odom_std_factor = 0.5;
-    Scalar gps_std = 1;
-    std::default_random_engine generator(1);
-    std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-    std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
-
-    //variables
-    Eigen::Vector2s odom_data;
-    Eigen::Vector2s gps_fix_reading;
-    Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
-    Eigen::Vector3s ground_truth_pose; //last true pose
-    Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
-    Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
-    clock_t t1, t2;
-    Scalar dt = 0.05;
-    TimeStamp ts(0);
-
-    // Wolf Tree initialization
-    Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
-    Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
-    Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
-    laser_1_pose << 1.2, 0, 0, 0; //laser 1
-    laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-
-    Problem problem(FRM_PO_2D);
-    SensorOdom2D* odom_sensor = new SensorOdom2D(std::make_shared<StateBlock>(odom_pose.head(2), true), std::make_shared<StateBlock>(odom_pose.tail(1), true), odom_std_factor, odom_std_factor);
-    SensorGPSFix* gps_sensor = new SensorGPSFix(std::make_shared<StateBlock>(gps_pose.head(2), true), std::make_shared<StateBlock>(gps_pose.tail(1), true), gps_std);
-    SensorLaser2D* laser_1_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_1_pose.head(2), true), std::make_shared<StateBlock>(laser_1_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-    SensorLaser2D* laser_2_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_2_pose.head(2), true), std::make_shared<StateBlock>(laser_2_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-    ProcessorTrackerLandmarkCorner* laser_1_processor = new ProcessorTrackerLandmarkCorner(laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2}), 3, 10);
-    ProcessorTrackerLandmarkCorner* laser_2_processor = new ProcessorTrackerLandmarkCorner(laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2}), 3, 10);
-    ProcessorOdom2D* odom_processor = new ProcessorOdom2D(1,1,100);
-    odom_sensor->addProcessor(odom_processor);
-    laser_1_sensor->addProcessor(laser_1_processor);
-    laser_2_sensor->addProcessor(laser_2_processor);
-    problem.addSensor(odom_sensor);
-    problem.addSensor(gps_sensor);
-    problem.addSensor(laser_1_sensor);
-    problem.addSensor(laser_2_sensor);
-    problem.setProcessorMotion(odom_processor);
-
-    CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
-
-    // Simulated robot
-    FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
-
-    // Initial pose
-    ground_truth_pose << 2, 8, 0;
-    ground_truth.head(3) = ground_truth_pose;
-    odom_trajectory.head(3) = ground_truth_pose;
-
-    // Origin Key Frame
-    FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
-
-    // Prior covariance
-    CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
-    origin_frame->addCapture(initial_covariance);
-    initial_covariance->process();
-
-    odom_processor->setOrigin(origin_frame);
-
-    // 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.minimizer_progress_to_stdout = false;
-    //    ceres_options.line_search_direction_type = ceres::LBFGS;
-    //    ceres_options.max_num_iterations = 100;
-    google::InitGoogleLogging(argv[0]);
-
-    CeresManager ceres_manager(&problem, ceres_options);
-    std::ofstream log_file, landmark_file;  //output file
-
-    //std::cout << "START TRAJECTORY..." << std::endl;
-    // START TRAJECTORY ============================================================================================
-    for (unsigned int step = 1; step < n_execution; step++)
-    {
-        // timestamp
-        ts = TimeStamp(step*dt);
-
-        //get init time
-        t2 = clock();
-
-        // ROBOT MOVEMENT ---------------------------
-        //std::cout << "ROBOT MOVEMENT..." << std::endl;
-        // moves the device position
-        t1 = clock();
-        ground_truth_pose = robot.motionCampus(step, odom_data(0), odom_data(1));
-        ground_truth.segment(step * 3, 3) = ground_truth_pose;
-
-        // ODOMETRY DATA -------------------------------------
-        // noisy odometry
-        odom_data(0) += distribution_odom(generator) * (odom_data(0) == 0 ? 1e-6 : odom_data(0));
-        odom_data(1) += distribution_odom(generator) * (odom_data(1) == 0 ? 1e-6 : odom_data(1));
-        // process odometry
-        odom_capture->setTimeStamp(TimeStamp(ts));
-        odom_capture->setData(odom_data);
-        odom_processor->process(odom_capture);
-        // odometry integration
-        odom_trajectory.segment(step * 3, 3) = problem.getCurrentState();
-
-        // LIDAR DATA ---------------------------
-        if (step % 3 == 0)
-        {
-            std::cout << "--PROCESS LIDAR 1 DATA..." << laser_1_sensor->id() << std::endl;
-            laser_1_processor->process(new CaptureLaser2D(ts, laser_1_sensor, robot.computeScan(1)));
-            std::cout << "--PROCESS LIDAR 2 DATA..." << laser_2_sensor->id() << std::endl;
-            laser_2_processor->process(new CaptureLaser2D(ts, laser_2_sensor, robot.computeScan(2)));
-        }
-
-        // GPS DATA ---------------------------
-        if (step % 5 == 0)
-        {
-            // compute GPS
-            gps_fix_reading  = ground_truth_pose.head<2>();
-            gps_fix_reading(0) += distribution_gps(generator);
-            gps_fix_reading(1) += distribution_gps(generator);
-            // process data
-            //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
-        }
-        mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // SOLVE OPTIMIZATION ---------------------------
-        //std::cout << "SOLVING..." << std::endl;
-        t1 = clock();
-        ceres::Solver::Summary summary = ceres_manager.solve();
-        //std::cout << summary.FullReport() << std::endl;
-        mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // COMPUTE COVARIANCES ---------------------------
-        //std::cout << "COMPUTING COVARIANCES..." << std::endl;
-        t1 = clock();
-        ceres_manager.computeCovariances();
-        mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // DRAWING STUFF ---------------------------
-        //std::cout << "RENDERING..." << std::endl;
-        t1 = clock();
-        if (step % 3 == 0)
-            robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
-            //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
-        mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // TIME MANAGEMENT ---------------------------
-        double total_t = ((double) clock() - t2) / CLOCKS_PER_SEC;
-        mean_times(6) += total_t;
-        if (total_t < 0.5)
-            usleep(500000 - 1e6 * total_t);
-
-//		std::cout << "\nTree after step..." << std::endl;
-    }
-
-    // DISPLAY RESULTS ============================================================================================
-    mean_times /= n_execution;
-    std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
-    std::cout << "  data generation:    " << mean_times(0) << std::endl;
-    std::cout << "  wolf managing:      " << mean_times(1) << std::endl;
-    std::cout << "  ceres managing:     " << mean_times(2) << std::endl;
-    std::cout << "  ceres optimization: " << mean_times(3) << std::endl;
-    std::cout << "  ceres covariance:   " << mean_times(4) << std::endl;
-    std::cout << "  results drawing:    " << mean_times(5) << std::endl;
-    std::cout << "  loop time:          " << mean_times(6) << std::endl;
-
-    //	std::cout << "\nTree before deleting..." << std::endl;
-
-    // Draw Final result -------------------------
-    robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
-
-    // Print Final result in a file -------------------------
-    // Vehicle poses
-    int i = 0;
-    Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
-    for (auto frame : *(problem.getTrajectory()->getFrameList()))
-    {
-        state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
-        i += 3;
-    }
-
-    // Landmarks
-    i = 0;
-    Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
-    for (auto landmark : *(problem.getMap()->getLandmarkList()))
-    {
-        landmarks.segment(i, 2) = landmark->getP()->getVector();
-        i += 2;
-    }
-
-    // Print log files
-    std::string filepath = getenv("HOME") + std::string("/Desktop/log_file_2.txt");
-    log_file.open(filepath, std::ofstream::out); //open log file
-
-    if (log_file.is_open())
-    {
-        log_file << 0 << std::endl;
-        for (unsigned int ii = 0; ii < n_execution; ii++)
-            log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
-        log_file.close(); //close log file
-        std::cout << std::endl << "Result file " << filepath << std::endl;
-    }
-    else
-        std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
-    std::string filepath2 = getenv("HOME") + std::string("/Desktop/landmarks_file_2.txt");
-    landmark_file.open(filepath2, std::ofstream::out); //open log file
-
-    if (landmark_file.is_open())
-    {
-        for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
-            landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
-        landmark_file.close(); //close log file
-        std::cout << std::endl << "Landmark file " << filepath << std::endl;
-    }
-    else
-        std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
-    std::cout << "Press any key for ending... " << std::endl << std::endl;
-    std::getchar();
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    //exit
-    return 0;
-}
diff --git a/demos/demo_ceres_2_lasers_polylines.cpp b/demos/demo_ceres_2_lasers_polylines.cpp
deleted file mode 100644
index d79bbfaf88853ee8ccec48f5c172ecce2b83e940..0000000000000000000000000000000000000000
--- a/demos/demo_ceres_2_lasers_polylines.cpp
+++ /dev/null
@@ -1,377 +0,0 @@
-//std includes
-#include "core/sensor/sensor_GPS_fix.h"
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//Ceres includes
-#include "glog/logging.h"
-
-//Wolf includes
-#include "core/problem/problem.h"
-#include "core/processor/processor_tracker_landmark_polyline.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// laserscanutils
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/laser_scan.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-#include "core/capture/capture_pose.h"
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-namespace wolf {
-class FaramoticsRobot
-{
-    public:
-
-        Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
-        vector < Cpose3d > devicePoses;
-        vector<float> scan1, scan2;
-        string modelFileName;
-        CrangeScan2D* myScanner;
-        CdynamicSceneRender* myRender;
-        Eigen::Vector3s ground_truth_pose_;
-        Eigen::Vector4s laser_1_pose_, laser_2_pose_;
-
-        FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
-            modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
-            laser_1_pose_(_laser_1_pose),
-            laser_2_pose_(_laser_2_pose)
-        {
-            devicePose.setPose(2, 8, 0.2, 0, 0, 0);
-            viewPoint.setPose(devicePose);
-            viewPoint.moveForward(10);
-            viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
-            viewPoint.moveForward(-15);
-            //glut initialization
-            faramotics::initGLUT(argc, argv);
-            //create a viewer for the 3D model and scan points
-            myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
-            myRender->loadAssimpModel(modelFileName, true); //with wireframe
-            //create scanner and load 3D model
-            myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);  //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
-            myScanner->loadAssimpModel(modelFileName);
-        }
-
-        //function travel around
-        Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
-        {
-            if (ii <= 120){         displacement_ = 0.1;    rotation_ = 0; }
-            else if (ii <= 170) {   displacement_ = 0.2;    rotation_ = 1.8 * M_PI / 180; }
-            else if (ii <= 220) {   displacement_ = 0;      rotation_ =-1.8 * M_PI / 180; }
-            else if (ii <= 310) {   displacement_ = 0.1;    rotation_ = 0; }
-            else if (ii <= 487) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-            else if (ii <= 600) {   displacement_ = 0.2;    rotation_ = 0; }
-            else if (ii <= 700) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-            else if (ii <= 780) {   displacement_ = 0;      rotation_ =-1.0 * M_PI / 180; }
-            else {                  displacement_ = 0.3;    rotation_ = 0; }
-
-            devicePose.moveForward(displacement_);
-            devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
-
-            // laser 1
-            laser1Pose.setPose(devicePose);
-            laser1Pose.moveForward(laser_1_pose_(0));
-            // laser 2
-            laser2Pose.setPose(devicePose);
-            laser2Pose.moveForward(laser_2_pose_(0));
-            laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-
-            devicePoses.push_back(devicePose);
-
-            ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-            return ground_truth_pose_;
-        }
-
-        ~FaramoticsRobot()
-        {
-            std::cout << "deleting render and scanner.." << std::endl;
-            delete myRender;
-            delete myScanner;
-            std::cout << "deleted!" << std::endl;
-        }
-
-        //compute scans
-        vector<float> computeScan(const int scan_id)
-        {
-            if (scan_id == 1)
-            {
-                scan1.clear();
-                myScanner->computeScan(laser1Pose, scan1);
-                return scan1;
-            }
-            else
-            {
-                scan2.clear();
-                myScanner->computeScan(laser2Pose, scan2);
-                return scan2;
-            }
-        }
-
-        void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
-        {
-            // detected corners
-            std::cout << "   drawCorners: " << feature_list.size() << std::endl;
-            std::vector<double> corner_vector;
-            corner_vector.reserve(2*feature_list.size());
-            for (auto corner : feature_list)
-            {
-                std::cout << "       corner " << corner->id() << std::endl;
-                corner_vector.push_back(corner->getMeasurement(0));
-                corner_vector.push_back(corner->getMeasurement(1));
-            }
-            myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
-
-            // landmarks
-            std::cout << "   drawLandmarks: " << landmark_list.size() << std::endl;
-            std::vector<double> landmark_vector;
-            landmark_vector.reserve(3*landmark_list.size());
-            for (auto landmark : landmark_list)
-            {
-                Scalar* position_ptr = landmark->getP()->get();
-                landmark_vector.push_back(*position_ptr); //x
-                landmark_vector.push_back(*(position_ptr + 1)); //y
-                landmark_vector.push_back(0.2); //z
-            }
-            myRender->drawLandmarks(landmark_vector);
-
-            // draw localization and sensors
-            estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
-            estimated_laser_1_pose.setPose(estimated_vehicle_pose);
-            estimated_laser_1_pose.moveForward(laser_1_pose_(0));
-            estimated_laser_2_pose.setPose(estimated_vehicle_pose);
-            estimated_laser_2_pose.moveForward(laser_2_pose_(0));
-            estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
-            myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
-            //Set view point and render the scene
-            //locate visualization view point, somewhere behind the device
-    //      viewPoint.setPose(devicePose);
-    //      viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-    //      viewPoint.moveForward(-5);
-            myRender->setViewPoint(viewPoint);
-            myRender->drawPoseAxis(devicePose);
-            myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
-            myRender->render();
-        }
-};
-}
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    std::cout << "\n============================================================\n";
-    std::cout << "========== 2D Robot with odometry and 2 LIDARs =============\n";
-
-    // USER INPUT ============================================================================================
-    if (argc != 2 || atoi(argv[1]) < 1 )
-    {
-        std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
-        std::cout << "     - NI is the number of iterations (NI > 0)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
-    // INITIALIZATION ============================================================================================
-    //init random generators
-    Scalar odom_std_factor = 0.5;
-    Scalar gps_std = 1;
-    std::default_random_engine generator(1);
-    std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-    std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
-
-    //variables
-    Eigen::Vector2s odom_data;
-    Eigen::Vector2s gps_fix_reading;
-    Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
-    Eigen::Vector3s ground_truth_pose; //last true pose
-    Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
-    Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
-    clock_t t1, t2;
-    Scalar dt = 0.05;
-    TimeStamp ts(0);
-
-    // Wolf Tree initialization
-    Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
-    Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
-    Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
-    laser_1_pose << 1.2, 0, 0, 0; //laser 1
-    laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-
-    Problem problem(FRM_PO_2D);
-    SensorOdom2D* odom_sensor = new SensorOdom2D(std::make_shared<StateBlock>(odom_pose.head(2), true), std::make_shared<StateBlock>(odom_pose.tail(1), true), odom_std_factor, odom_std_factor);
-    SensorGPSFix* gps_sensor = new SensorGPSFix(std::make_shared<StateBlock>(gps_pose.head(2), true), std::make_shared<StateBlock>(gps_pose.tail(1), true), gps_std);
-    SensorLaser2D* laser_1_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_1_pose.head(2), true), std::make_shared<StateBlock>(laser_1_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-    SensorLaser2D* laser_2_sensor = new SensorLaser2D(std::make_shared<StateBlock>(laser_2_pose.head(2), true), std::make_shared<StateBlock>(laser_2_pose.tail(1), true), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-
-    wolf::ProcessorParamsPolyline laser_processor_params;
-    laser_processor_params.line_finder_params = laserscanutils::LineFinderIterativeParams({0.1, 5, 1, 2});
-    laser_processor_params.new_features_th = 3;
-    laser_processor_params.loop_frames_th = 10;
-    laser_processor_params.time_tolerance = 0.1;
-    laser_processor_params.position_error_th = 0.5;
-    ProcessorTrackerLandmarkPolyline* laser_1_processor = new ProcessorTrackerLandmarkPolyline(laser_processor_params);
-    ProcessorTrackerLandmarkPolyline* laser_2_processor = new ProcessorTrackerLandmarkPolyline(laser_processor_params);
-    ProcessorOdom2D* odom_processor = new ProcessorOdom2D(1,1,100);
-    odom_sensor->addProcessor(odom_processor);
-    laser_1_sensor->addProcessor(laser_1_processor);
-    laser_2_sensor->addProcessor(laser_2_processor);
-    problem.addSensor(odom_sensor);
-    problem.addSensor(gps_sensor);
-    problem.addSensor(laser_1_sensor);
-    problem.addSensor(laser_2_sensor);
-    problem.setProcessorMotion(odom_processor);
-
-    CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
-
-    // Simulated robot
-    FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
-
-    // Initial pose
-    ground_truth_pose << 2, 8, 0;
-    ground_truth.head(3) = ground_truth_pose;
-    odom_trajectory.head(3) = ground_truth_pose;
-
-    // Origin Key Frame
-    FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
-
-    // Prior covariance
-    CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
-    origin_frame->addCapture(initial_covariance);
-    initial_covariance->process();
-
-    odom_processor->setOrigin(origin_frame);
-
-    // 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.minimizer_progress_to_stdout = false;
-    //    ceres_options.line_search_direction_type = ceres::LBFGS;
-    //    ceres_options.max_num_iterations = 100;
-    google::InitGoogleLogging(argv[0]);
-
-    CeresManager ceres_manager(&problem, ceres_options);
-    std::ofstream log_file, landmark_file;  //output file
-
-    //std::cout << "START TRAJECTORY..." << std::endl;
-    // START TRAJECTORY ============================================================================================
-    for (unsigned int step = 1; step < n_execution; step++)
-    {
-        // timestamp
-        ts = TimeStamp(step*dt);
-
-        //get init time
-        t2 = clock();
-
-        // ROBOT MOVEMENT ---------------------------
-        //std::cout << "ROBOT MOVEMENT..." << std::endl;
-        // moves the device position
-        t1 = clock();
-        ground_truth_pose = robot.motionCampus(step, odom_data(0), odom_data(1));
-        ground_truth.segment(step * 3, 3) = ground_truth_pose;
-
-        // ODOMETRY DATA -------------------------------------
-        // noisy odometry
-        odom_data(0) += distribution_odom(generator) * (odom_data(0) == 0 ? 1e-6 : odom_data(0));
-        odom_data(1) += distribution_odom(generator) * (odom_data(1) == 0 ? 1e-6 : odom_data(1));
-        // process odometry
-        odom_capture->setTimeStamp(TimeStamp(ts));
-        odom_capture->setData(odom_data);
-        odom_processor->process(odom_capture);
-        // odometry integration
-        odom_trajectory.segment(step * 3, 3) = problem.getCurrentState();
-
-        // LIDAR DATA ---------------------------
-        if (step % 3 == 0)
-        {
-            std::cout << "--PROCESS LIDAR 1 DATA..." << laser_1_sensor->id() << std::endl;
-            laser_1_processor->process(new CaptureLaser2D(ts, laser_1_sensor, robot.computeScan(1)));
-            std::cout << "--PROCESS LIDAR 2 DATA..." << laser_2_sensor->id() << std::endl;
-            laser_2_processor->process(new CaptureLaser2D(ts, laser_2_sensor, robot.computeScan(2)));
-        }
-
-        // GPS DATA ---------------------------
-        if (step % 5 == 0)
-        {
-            // compute GPS
-            gps_fix_reading  = ground_truth_pose.head<2>();
-            gps_fix_reading(0) += distribution_gps(generator);
-            gps_fix_reading(1) += distribution_gps(generator);
-            // process data
-            //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
-        }
-        mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // SOLVE OPTIMIZATION ---------------------------
-        std::cout << "SOLVING..." << std::endl;
-        t1 = clock();
-        std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
-        std::cout << summary << std::endl;
-        mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // COMPUTE COVARIANCES ---------------------------
-        std::cout << "COMPUTING COVARIANCES..." << std::endl;
-        t1 = clock();
-        //ceres_manager.computeCovariances();
-        mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // DRAWING STUFF ---------------------------
-        std::cout << "RENDERING..." << std::endl;
-        t1 = clock();
-//        if (step % 3 == 0)
-//            robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
-            //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
-        mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // TIME MANAGEMENT ---------------------------
-        double total_t = ((double) clock() - t2) / CLOCKS_PER_SEC;
-        mean_times(6) += total_t;
-        if (total_t < 0.5)
-            usleep(500000 - 1e6 * total_t);
-
-//		std::cout << "\nTree after step..." << std::endl;
-    }
-
-    // DISPLAY RESULTS ============================================================================================
-    mean_times /= n_execution;
-    std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
-    std::cout << "  data generation:    " << mean_times(0) << std::endl;
-    std::cout << "  wolf managing:      " << mean_times(1) << std::endl;
-    std::cout << "  ceres managing:     " << mean_times(2) << std::endl;
-    std::cout << "  ceres optimization: " << mean_times(3) << std::endl;
-    std::cout << "  ceres covariance:   " << mean_times(4) << std::endl;
-    std::cout << "  results drawing:    " << mean_times(5) << std::endl;
-    std::cout << "  loop time:          " << mean_times(6) << std::endl;
-
-    std::cout << "\nTree before deleting..." << std::endl;
-
-        std::cout << "Press any key for ending... " << std::endl << std::endl;
-    std::getchar();
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    //exit
-    return 0;
-}
diff --git a/demos/demo_diff_drive.cpp b/demos/demo_diff_drive.cpp
deleted file mode 100644
index 5585fe035ca81d0b7ae8442a03cb3ff7c790680a..0000000000000000000000000000000000000000
--- a/demos/demo_diff_drive.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-/**
- * \file test_diff_drive.cpp
- *
- *  Created on: Oct 26, 2017
- *  \author: Jeremie Deray
- */
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_diff_drive.h"
-#include "core/capture/capture_wheel_joint_position.h"
-#include "core/processor/processor_diff_drive.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-void getOdom2DData(std::ifstream& _stream, wolf::Scalar& _stamp, Eigen::Vector2s& _data)
-{
-  /*
-   * Data are logged as follows :
-   *
-   *  header:
-   *    seq: xxx
-   *    stamp:
-   *      secs: xxx
-   *      nsecs: xxx
-   *    frame_id: ''
-   *  twist:
-   *    linear:
-   *      x: 0.0
-   *      y: 0.0
-   *      z: 0.0
-   *    angular:
-   *      x: 0.0
-   *      y: 0.0
-   *      z: 0.0
-   * ---
-   */
-
-  std::string dummy;
-
-  getline(_stream, dummy); // header:
-  getline(_stream, dummy); // seq: xxx
-  getline(_stream, dummy); // stamp:
-  getline(_stream, dummy); // secs: xxx
-
-  // Find secs
-  std::string sub("secs: ");
-  std::string::size_type i = dummy.find(sub);
-  dummy.erase(i, sub.length());
-
-  _stamp = std::stod(dummy);
-
-  // Find nsecs
-  getline(_stream, dummy); // nsecs: xxx
-  sub = "nsecs: ";
-  i = dummy.find(sub);
-  dummy.erase(i, sub.length());
-
-  _stamp += std::stod(dummy) * wolf::Scalar(1e-9);
-
-  getline(_stream, dummy); // frame_id: ''
-  getline(_stream, dummy); // twist:
-  getline(_stream, dummy); // linear:
-  getline(_stream, dummy); // x: 0.0
-
-  sub = "x: ";
-  i = dummy.find(sub);
-  dummy.erase(i, sub.length());
-
-  _data(0) = std::stod(dummy);
-
-  getline(_stream, dummy); // y: 0.0
-  getline(_stream, dummy); // z: 0.0
-  getline(_stream, dummy); // angular:
-  getline(_stream, dummy); // x: 0.0
-  getline(_stream, dummy); // y: 0.0
-  getline(_stream, dummy); // z: 0.0
-
-  sub = "z: ";
-  i = dummy.find(sub);
-  dummy.erase(i, sub.length());
-
-  _data(1) = std::stod(dummy);
-
-  getline(_stream, dummy); // ---
-}
-
-void readWheelData(std::ifstream &_stream, Eigen::Vector2s &_data)
-{
-  /*
-   * left_wheel_joint_actual_position: [x]
-   * right_wheel_joint_actual_position: [x]
-   * ---
-   */
-
-  std::string dummy;
-  std::string l_brac("[");
-  std::string r_brac("]");
-
-  getline(_stream, dummy);
-
-  unsigned first = dummy.find(l_brac);
-  unsigned last  = dummy.find(r_brac);
-
-  //std::cout << "READING : " << dummy.substr(first+1, last-first-1) << std::endl;
-
-  _data(0) = std::stod(dummy.substr(first+1, last-first-1));
-
-  getline(_stream, dummy);
-
-  first = dummy.find(l_brac);
-  last  = dummy.find(r_brac);
-
-  //std::cout << "READING : " << dummy.substr(first+1, last-first-1) << std::endl;
-
-  _data(1) = std::stod(dummy.substr(first+1 , last-first-1));
-
-  getline(_stream, dummy);
-}
-
-bool WHEEL_DATA = true;
-bool VERBOSE = false;
-
-int main(int argc, char** argv)
-{
-  using namespace wolf;
-
-  WOLF_INFO("==================== diff drive test ======================");
-
-  //load files containing data
-  std::ifstream data_file;
-  const char * filename;
-
-  if (argc < 2)
-  {
-   WOLF_ERROR("Missing input argument! :"
-              " needs 2 arguments (path to data file & data type "
-              "- velocities or wheel positions).");
-    return EXIT_FAILURE;
-  }
-  else
-  {
-    filename   = argv[1];
-    if (argc >= 3) WHEEL_DATA = std::stoi(argv[2]);
-
-    data_file.open(filename);
-
-    WOLF_INFO("Data file: ", filename);
-
-    if (!data_file.is_open())
-    {
-      WOLF_ERROR("Failed to open data files... Exiting");
-      return EXIT_FAILURE;
-    }
-  }
-
-  // Wolf problem
-  ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
-
-  const std::string sensor_name("Main Odometer");
-  Eigen::VectorXs extrinsics(3);
-  extrinsics << 0, 0, 0;
-
-  IntrinsicsBasePtr intrinsics = std::make_shared<IntrinsicsDiffDrive>();
-
-  IntrinsicsDiffDrivePtr intrinsics_diff_drive =
-      std::static_pointer_cast<IntrinsicsDiffDrive>(intrinsics);
-
-  intrinsics_diff_drive->left_radius_  = 0.1;
-  intrinsics_diff_drive->right_radius_ = 0.1;
-  intrinsics_diff_drive->separation_   = 0.3517;
-
-  intrinsics_diff_drive->model_        = wolf::DiffDriveModel::Three_Factor_Model;
-  intrinsics_diff_drive->factors_      = Eigen::Vector3s(1,1,1);
-
-  intrinsics_diff_drive->left_resolution_  = 0.0001653; // [rad]
-  intrinsics_diff_drive->right_resolution_ = 0.0001653; // [rad]
-
-  intrinsics_diff_drive->left_gain_        = 0.01;
-  intrinsics_diff_drive->right_gain_       = 0.01;
-
-  // Time and data variables
-  TimeStamp t;
-  Scalar stamp_secs(0);
-//  Scalar period_secs(0.010); //100Hz
-  Scalar period_secs(0.020); //50Hz
-  Eigen::Vector2s data_; data_ << 0,0;
-
-  const auto scalar_max = std::numeric_limits<Scalar>::max();
-
-  ProcessorParamsDiffDrivePtr processor_params = std::make_shared<ProcessorParamsDiffDrive>();
-  processor_params->time_tolerance  = period_secs/2;
-  processor_params->angle_turned    = scalar_max;
-  processor_params->dist_traveled   = scalar_max;
-  processor_params->max_time_span   = scalar_max;
-  processor_params->max_buff_length = 999;
-  processor_params->unmeasured_perturbation_std = 0.0001;
-
-  SensorBasePtr sensor_ptr =
-      wolf_problem_ptr_->installSensor("DIFF DRIVE", sensor_name, extrinsics, intrinsics);
-
-  WOLF_INFO("Sensor 'DIFF DRIVE' installed.");
-
-  auto diff_drive_sensor_ptr = std::static_pointer_cast<SensorDiffDrive>(sensor_ptr);
-
-  wolf_problem_ptr_->installProcessor("DIFF DRIVE", "Diffential Drive processor", sensor_ptr, processor_params);
-
-  WOLF_INFO("Processor 'DIFF DRIVE' installed.");
-
-  // Get initial wheel data
-  if (WHEEL_DATA)
-    readWheelData(data_file, data_);
-  else
-    getOdom2DData(data_file, stamp_secs, data_);
-
-  t.set(stamp_secs);
-  auto processor_diff_drive_ptr =
-      std::static_pointer_cast<ProcessorDiffDrive>(wolf_problem_ptr_->getProcessorMotion());
-  processor_diff_drive_ptr->setTimeTolerance(period_secs/2); // overwrite time tolerance based on new evidence
-
-  // Set the origin
-  // Create one capture to store the Odometry data.
-  std::shared_ptr<CaptureWheelJointPosition> data_ptr =
-      std::make_shared<CaptureWheelJointPosition>(t, sensor_ptr, data_, nullptr);
-
-  WOLF_INFO("Process first capture.");
-
-  diff_drive_sensor_ptr->process(data_ptr);
-
-  // main loop
-  clock_t begin = clock();
-
-  while (!data_file.eof())
-  {
-    // read new data
-    if (WHEEL_DATA)
-    {
-      readWheelData(data_file, data_);
-      stamp_secs += period_secs;
-    }
-    else
-      getOdom2DData(data_file, stamp_secs, data_);
-
-    t.set(stamp_secs);
-
-    data_ptr = std::make_shared<CaptureWheelJointPosition>(t, sensor_ptr, data_, nullptr);
-
-    // process data in capture
-    diff_drive_sensor_ptr->process(data_ptr);
-
-    WOLF_INFO_COND(VERBOSE, "At stamp ", stamp_secs,
-                   " state ", processor_diff_drive_ptr->getCurrentState().transpose());
-  }
-
-  data_file.close();
-
-  double elapsed_secs = double(clock() - begin) / CLOCKS_PER_SEC;
-
-  // Final state
-  WOLF_INFO("----------------------------------------- "
-            "Integration results "
-            "-----------------------------------------");
-
-  WOLF_INFO("Integrated delta: " , /* std::fixed , std::setprecision(3),*/
-            wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose());
-  WOLF_INFO("Integrated state: " , /*std::fixed , std::setprecision(3),*/
-            wolf_problem_ptr_->getProcessorMotion()->getCurrentState().transpose());
-  WOLF_INFO("Integrated std  : " , /*std::fixed , std::setprecision(3),*/
-            (wolf_problem_ptr_->getProcessorMotion()->getMotion().
-                delta_integr_cov_.diagonal().transpose()).array().sqrt());
-
-  // Print statistics
-  TimeStamp t0, tf;
-  t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
-  tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-
-  double N = (double)wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-
-  WOLF_INFO("t0        : " , t0.get()               , " secs");
-  WOLF_INFO("tf        : " , tf.get()               , " secs");
-  WOLF_INFO("duration  : " , tf-t0                  , " secs");
-  WOLF_INFO("N samples : " , N                               );
-  WOLF_INFO("frequency : " , (N-1)/(tf-t0)          , " Hz"  );
-  WOLF_INFO("CPU time  : " , elapsed_secs           , " s"   );
-  WOLF_INFO("s/integr  : " , elapsed_secs/(N-1)*1e6 , " us"  );
-  WOLF_INFO("integr/s  : " , (N-1)/elapsed_secs     , " ips" );
-
-  return 0;
-}
diff --git a/demos/demo_eigen_quaternion.cpp b/demos/demo_eigen_quaternion.cpp
deleted file mode 100644
index 1e85039d7d5a25c6320c74d0d1914c372a9220f6..0000000000000000000000000000000000000000
--- a/demos/demo_eigen_quaternion.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-
-//std
-#include <iostream>
-
-//Eigen
-#include <eigen3/Eigen/Geometry>
-
-//Wolf
-#include "core/common/wolf.h"
-
-int main()
-{
-    using namespace wolf;
-
-    std::cout << std::endl << "Eigen Quatenrnion test" << std::endl;
-    
-    Scalar q1[4]; 
-    Eigen::Map<Eigen::Quaternions> q1_map(q1);
-    
-    //try to find out how eigen sorts storage (real part tail or head ? )
-    q1_map.w() = 1; 
-    q1_map.x() = 2; 
-    q1_map.y() = 3; 
-    q1_map.z() = 4; 
-    std::cout << "q1[0]=" << q1[0] << "; q1_map.x()=" << q1_map.x() << std::endl;
-    std::cout << "q1[1]=" << q1[1] << "; q1_map.y()=" << q1_map.y() << std::endl;
-    std::cout << "q1[2]=" << q1[2] << "; q1_map.z()=" << q1_map.z() << std::endl;
-    std::cout << "q1[3]=" << q1[3] << "; q1_map.w()=" << q1_map.w() << std::endl;
-    std::cout << std::endl << "RESULT: Eigen stores REAL part in the LAST memory position of the quaternion." << std::endl;
-    
-    std::cout << std::endl << "End of Eigen Quatenrnion test" << std::endl;
-    return 0;
-}
-
diff --git a/demos/demo_eigen_template.cpp b/demos/demo_eigen_template.cpp
deleted file mode 100644
index 47e5aa4191e1d680b4cbc5a8702d7c55f74ed52e..0000000000000000000000000000000000000000
--- a/demos/demo_eigen_template.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/**
- * \file test_eigen_template.cpp
- *
- *  Created on: Sep 12, 2016
- *      \author: jsola
- */
-
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-#include <iostream>
-
-template <int Size, int DesiredSize>
-struct StaticSizeCheck {
-        template <typename T>
-        StaticSizeCheck(const T&) {
-            static_assert(Size == DesiredSize, "Static sizes do not match");
-        }
-};
-
-template <int DesiredSize>
-struct StaticSizeCheck<Eigen::Dynamic, DesiredSize> {
-        template <typename T>
-        StaticSizeCheck(const T& t) {
-            assert(t == DesiredSize && "Dynamic sizes do not match");
-        }
-};
-
-template <int DesiredR, int DesiredC>
-struct MatrixSizeCheck {
-        template <typename T>
-        static void check(const T& t) {
-            StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
-            StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
-        }
-};
-
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v){
-
-    MatrixSizeCheck<3, 1>::check(_v);
-
-    Eigen::Quaternion<typename Derived::Scalar> q;
-    typename Derived::Scalar angle = _v.norm();
-    typename Derived::Scalar angle_half = angle/2.0;
-    if (angle > 1e-8)
-    {
-        q.w() = cos(angle_half);
-        q.vec() = _v / angle * sin(angle_half);
-        return q;
-    }
-    else
-    {
-        q.w() = cos(angle_half);
-        q.vec() = _v * ( (typename Derived::Scalar)0.5 - angle_half*angle_half/(typename Derived::Scalar)12.0 ); // see the Taylor series of sinc(x) ~ 1 - x^2/3!, and have q.vec = v/2 * sinc(angle_half)
-        return q;
-    }
-}
-
-int main(void)
-{
-    using namespace Eigen;
-
-    VectorXd x(10);
-    x << 1,2,3,4,5,6,7,8,9,10;
-
-    Quaterniond q;
-    Map<Quaterniond> qm(x.data()+5);
-
-    // Static vector
-    Vector3d v;
-    v << 1,2,3;
-    q  = v2q(v);
-    qm = v2q(v);
-    std::cout << q.coeffs().transpose() << std::endl;
-    std::cout << qm.coeffs().transpose() << std::endl;
-
-    // Dynamic matrix
-    Matrix<double, Dynamic, Dynamic> M(3,1);
-    M << 1, 2, 3;
-    q  = v2q(M);
-    std::cout << q.coeffs().transpose() << std::endl;
-
-    // Dynamic vector segment
-    x << 1,2,3,4,5,6,7,8,9,10;
-    q  = v2q(x.head(3));
-    std::cout << q.coeffs().transpose() << std::endl;
-
-    // Map over dynamic vector
-    Map<VectorXd> m(x.data(), 3);
-    q  = v2q(m);
-    std::cout << q.coeffs().transpose() << std::endl;
-
-    // Float scalar
-    Vector3f vf;
-    Quaternionf qf;
-    vf << 1,2,3;
-    qf = v2q(vf);
-    std::cout << qf.coeffs().transpose() << std::endl;
-
-    //    // Static assert at compile time
-    //    Vector2d v2;
-    //    q= v2q(v2);
-    //    std::cout << q.coeffs().transpose() << std::endl;
-
-}
diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp
deleted file mode 100644
index 36e6bfa52385e37e51a8d1616e3c20640f35b371..0000000000000000000000000000000000000000
--- a/demos/demo_factor_imu.cpp
+++ /dev/null
@@ -1,279 +0,0 @@
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/capture/capture_pose.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//#define DEBUG_RESULTS
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    std::cout << std::endl << "==================== test_factor_imu ======================" << std::endl;
-
-    bool c0(false), c1(false);// c2(true), c3(true), c4(true);
-    // Wolf problem
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
-    Eigen::VectorXs IMU_extrinsics(7);
-    IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-    SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, shared_ptr<IntrinsicsBase>());
-    wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
-    // Ceres wrappers
-    ceres::Solver::Options ceres_options;
-    ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-    ceres_options.max_line_search_step_contraction = 1e-3;
-    ceres_options.max_num_iterations = 1e4;
-    CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
-
-    // Time and data variables
-    TimeStamp t;
-    Eigen::Vector6s data_;
-    Scalar mpu_clock = 0;
-
-    t.set(mpu_clock);
-
-    // Set the origin
-    Eigen::VectorXs x0(16);
-    x0 << 0,0,0,  0,0,0,1,  0,0,0,  0,0,.001,  0,0,.002; // Try some non-zero biases
-    wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t); //this also creates a keyframe at origin
-    wolf_problem_ptr_->getTrajectory()->getFrameList().front()->fix(); //fix the keyframe at origin
-
-    TimeStamp ts(0);
-    Eigen::VectorXs origin_state = x0;
-    
-    // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
-    CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
-    imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
-
-    // set variables
-    using namespace std;
-    Eigen::VectorXs state_vec;
-    Eigen::VectorXs delta_preint;
-    //FrameIMUPtr last_frame;
-    Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
-
-    //process data
-    mpu_clock = 0.001003;
-    //data_ << 0.579595, -0.143701, 9.939331, 0.127445, 0.187814, -0.055003;
-    data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
-    t.set(mpu_clock);
-    // assign data to capture
-    imu_ptr->setData(data_);
-    imu_ptr->setTimeStamp(t);
-    // process data in capture
-    sensor_ptr->process(imu_ptr);
-
-    if(c0){
-    /// ******************************************************************************************** ///
-    /// factor creation
-    //create FrameIMU
-    ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
-    wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
-        //create a feature
-    delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
-    delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
-    std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
-    feat_imu->setCapture(imu_ptr);
-
-        //create a factorIMU
-    FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
-    feat_imu->addFactor(factor_imu);
-    last_frame->addConstrainedBy(factor_imu);
-
-    FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
-    Eigen::Matrix<wolf::Scalar, 10, 1> expect;
-    Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
-    Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
-    Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
-    Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
-    Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
-    Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
-    Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
-    Eigen::Matrix<wolf::Scalar, 9, 1> residu;
-    residu << 0,0,0,  0,0,0,  0,0,0;
-    
-    factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, current_frame_p, current_frame_o, current_frame_v, expect);
-    std::cout << "expectation : " << expect.transpose() << std::endl;
-
-    factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
-    std::cout << "residuals : " << residu.transpose() << std::endl;
-
-    //reset origin of motion to new frame
-    wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
-    imu_ptr->setFrame(last_frame);
-    }
-    /// ******************************************************************************************** ///
-
-    mpu_clock = 0.002135;
-    //data_ << 0.581990, -0.191602, 10.071057, 0.136836, 0.203912, -0.057686;
-	data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
-    t.set(mpu_clock);
-    imu_ptr->setData(data_);
-    imu_ptr->setTimeStamp(t);
-    sensor_ptr->process(imu_ptr);
-
-    mpu_clock = 0.003040;
-    //data_ << 0.596360, -0.225132, 10.205178, 0.154276, 0.174399, -0.036221;
-    data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
-    t.set(mpu_clock);
-    imu_ptr->setData(data_);
-    imu_ptr->setTimeStamp(t);
-    sensor_ptr->process(imu_ptr);
-
-    if(c1){
-    /// ******************************************************************************************** ///
-    /// factor creation
-    //create FrameIMU
-    ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
-    wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
-        //create a feature
-    delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
-    delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
-    std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
-    feat_imu->setCapture(imu_ptr);
-
-        //create a factorIMU
-    FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
-    feat_imu->addFactor(factor_imu);
-    last_frame->addConstrainedBy(factor_imu);
-
-    FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
-    Eigen::Matrix<wolf::Scalar, 10, 1> expect;
-    Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
-    Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
-    Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
-    Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
-    Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
-    Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
-    Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
-    Eigen::Matrix<wolf::Scalar, 9, 1> residu;
-    residu << 0,0,0,  0,0,0,  0,0,0;
-    
-    factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
-    std::cout << "expectation : " << expect.transpose() << std::endl;
-
-    factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
-    std::cout << "residuals : " << residu.transpose() << std::endl;
-
-    //reset origin of motion to new frame
-    wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
-    imu_ptr->setFrame(last_frame);
-    }
-
-    mpu_clock = 0.004046;
-    //data_ << 0.553250, -0.203577, 10.324929, 0.128787, 0.156959, -0.044270;
-    data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
-    t.set(mpu_clock);
-    imu_ptr->setData(data_);
-    imu_ptr->setTimeStamp(t);
-    sensor_ptr->process(imu_ptr);
-
-    mpu_clock = 0.005045;
-    //data_ << 0.548459, -0.184417, 10.387200, 0.083175, 0.120738, -0.026831;
-    data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
-    t.set(mpu_clock);
-    imu_ptr->setData(data_);
-    imu_ptr->setTimeStamp(t);
-    sensor_ptr->process(imu_ptr);
-
-    //create the factor
-        //create FrameIMU
-    ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
-    wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
-        //create a feature
-    delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
-    delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
-    std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
-    feat_imu->setCapture(imu_ptr);
-
-        //create a factorIMU
-    FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
-    feat_imu->addFactor(factor_imu);
-    last_frame->addConstrainedBy(factor_imu);
-
-    FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
-    Eigen::Matrix<wolf::Scalar, 10, 1> expect;
-    Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
-    Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
-    Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
-    Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
-    Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
-    Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
-    Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
-    Eigen::Matrix<wolf::Scalar, 9, 1> residu;
-    residu << 0,0,0,  0,0,0,  0,0,0;
-    
-    factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
-    std::cout << "expectation : " << expect.transpose() << std::endl;
-
-    factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
-    std::cout << "residuals : " << residu.transpose() << std::endl;
-
-    if(wolf_problem_ptr_->check(1)){
-        wolf_problem_ptr_->print(4,1,1,1);
-    }
-
-    ///having a look at covariances
-    Eigen::MatrixXs predelta_cov;
-    predelta_cov.resize(9,9);
-    predelta_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
-    //std::cout << "predelta_cov : \n" << predelta_cov << std::endl; 
-
-        ///Optimization
-    // PRIOR
-    //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front();
-    wolf_problem_ptr_->getProcessorMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front());
-    //SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
-    //CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01);
-    //first_frame->addCapture(initial_covariance);
-    //initial_covariance->process();
-    //std::cout << "initial covariance: factor " << initial_covariance->getFeatureList().front()->getConstrainedByList().front()->id() << std::endl << initial_covariance->getFeatureList().front()->getMeasurementCovariance() << std::endl;
-
-    // COMPUTE COVARIANCES
-    std::cout << "computing covariances..." << std::endl;
-    ceres_manager_wolf_diff->computeCovariances(ALL_MARGINALS);//ALL_MARGINALS, ALL
-    std::cout << "computed!" << std::endl;
-
-    /*
-    // SOLVING PROBLEMS
-    ceres::Solver::Summary summary_diff;
-    std::cout << "solving..." << std::endl;
-    Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
-    summary_diff = ceres_manager_wolf_diff->solve();
-    Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
-    std::cout << " prev_wolf_state : " << prev_wolf_state.transpose() << "\n post_wolf_state : " << post_wolf_state.transpose() << std::endl;
-    //std::cout << summary_wolf_diff.BriefReport() << std::endl;
-    std::cout << "solved!" << std::endl;
-    */
-
-    /*
-    std::cout << "WOLF AUTO DIFF" << std::endl;
-    std::cout << "Jacobian evaluation: " << summary_wolf.jacobian_evaluation_time_in_seconds << std::endl;
-    std::cout << "Total time: " << summary_wolf.total_time_in_seconds << std::endl;
-    */
-
-    return 0;
-}
diff --git a/demos/demo_factor_odom_3D.cpp b/demos/demo_factor_odom_3D.cpp
deleted file mode 100644
index be7a81c4f454a8c3c6a4bc621b9de8cb3ef39a0d..0000000000000000000000000000000000000000
--- a/demos/demo_factor_odom_3D.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-/*
- * test_factor_odom_3D.cpp
- *
- *  Created on: Oct 7, 2016
- *      Author: jsola
- */
-
-#include "core/factor/factor_odom_3D.h"
-
-namespace wolf
-{
-
-} /* namespace wolf */
diff --git a/demos/demo_faramotics_simulation.cpp b/demos/demo_faramotics_simulation.cpp
deleted file mode 100644
index 4414520dc1fc0edf2260ad4cf416ee3ddd2ea56d..0000000000000000000000000000000000000000
--- a/demos/demo_faramotics_simulation.cpp
+++ /dev/null
@@ -1,244 +0,0 @@
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// Eigen includes
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-// wolf
-#include "core/common/wolf.h"
-#include "core/feature/feature_base.h"
-#include "core/landmark/landmark_base.h"
-#include "core/state_block/state_block.h"
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-namespace wolf {
-class FaramoticsRobot
-{
-    public:
-
-        Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
-        vector < Cpose3d > devicePoses;
-        vector<float> scan1, scan2;
-        string modelFileName;
-        CrangeScan2D* myScanner;
-        CdynamicSceneRender* myRender;
-        Eigen::Vector3s ground_truth_pose_;
-        Eigen::Vector4s laser_1_pose_, laser_2_pose_;
-
-        FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
-            modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
-            laser_1_pose_(_laser_1_pose),
-            laser_2_pose_(_laser_2_pose)
-        {
-            devicePose.setPose(2, 8, 0.2, 0, 0, 0);
-            viewPoint.setPose(devicePose);
-            viewPoint.moveForward(10);
-            viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
-            viewPoint.moveForward(-15);
-            //glut initialization
-            faramotics::initGLUT(argc, argv);
-            //create a viewer for the 3D model and scan points
-            myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
-            myRender->loadAssimpModel(modelFileName, true); //with wireframe
-            //create scanner and load 3D model
-            myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);  //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
-            myScanner->loadAssimpModel(modelFileName);
-        }
-
-        //function travel around
-        Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
-        {
-            if (ii <= 120){         displacement_ = 0.1;    rotation_ = 0; }
-            else if (ii <= 170) {   displacement_ = 0.2;    rotation_ = 1.8 * M_PI / 180; }
-            else if (ii <= 220) {   displacement_ = 0;      rotation_ =-1.8 * M_PI / 180; }
-            else if (ii <= 310) {   displacement_ = 0.1;    rotation_ = 0; }
-            else if (ii <= 487) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-            else if (ii <= 600) {   displacement_ = 0.2;    rotation_ = 0; }
-            else if (ii <= 700) {   displacement_ = 0.1;    rotation_ =-1.0 * M_PI / 180; }
-            else if (ii <= 780) {   displacement_ = 0;      rotation_ =-1.0 * M_PI / 180; }
-            else {                  displacement_ = 0.3;    rotation_ = 0; }
-
-            devicePose.moveForward(displacement_);
-            devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
-
-            // laser 1
-            laser1Pose.setPose(devicePose);
-            laser1Pose.moveForward(laser_1_pose_(0));
-            // laser 2
-            laser2Pose.setPose(devicePose);
-            laser2Pose.moveForward(laser_2_pose_(0));
-            laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-
-            devicePoses.push_back(devicePose);
-
-            ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-            return ground_truth_pose_;
-        }
-
-        ~FaramoticsRobot()
-        {
-            std::cout << "deleting render and scanner.." << std::endl;
-            delete myRender;
-            delete myScanner;
-            std::cout << "deleted!" << std::endl;
-        }
-
-        //compute scans
-        vector<float> computeScan(const int scan_id)
-        {
-            if (scan_id == 1)
-            {
-                scan1.clear();
-                myScanner->computeScan(laser1Pose, scan1);
-                return scan1;
-            }
-            else
-            {
-                scan2.clear();
-                myScanner->computeScan(laser2Pose, scan2);
-                return scan2;
-            }
-        }
-
-        void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
-        {
-            // detected corners
-            //std::cout << "   drawCorners: " << feature_list.size() << std::endl;
-            std::vector<double> corner_vector;
-            corner_vector.reserve(2*feature_list.size());
-            for (auto corner : feature_list)
-            {
-                //std::cout << "       corner " << corner->id() << std::endl;
-                corner_vector.push_back(corner->getMeasurement(0));
-                corner_vector.push_back(corner->getMeasurement(1));
-            }
-            myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
-
-            // landmarks
-            //std::cout << "   drawLandmarks: " << landmark_list.size() << std::endl;
-            std::vector<double> landmark_vector;
-            landmark_vector.reserve(3*landmark_list.size());
-            for (auto landmark : landmark_list)
-            {
-                Scalar* position_ptr = landmark->getP()->get();
-                landmark_vector.push_back(*position_ptr); //x
-                landmark_vector.push_back(*(position_ptr + 1)); //y
-                landmark_vector.push_back(0.2); //z
-            }
-            myRender->drawLandmarks(landmark_vector);
-
-            // draw localization and sensors
-            estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
-            estimated_laser_1_pose.setPose(estimated_vehicle_pose);
-            estimated_laser_1_pose.moveForward(laser_1_pose_(0));
-            estimated_laser_2_pose.setPose(estimated_vehicle_pose);
-            estimated_laser_2_pose.moveForward(laser_2_pose_(0));
-            estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
-            myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
-            //Set view point and render the scene
-            //locate visualization view point, somewhere behind the device
-    //      viewPoint.setPose(devicePose);
-    //      viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-    //      viewPoint.moveForward(-5);
-            myRender->setViewPoint(viewPoint);
-            myRender->drawPoseAxis(devicePose);
-            myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
-            myRender->render();
-        }
-};
-}
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    std::cout << "\n============================================================\n";
-    std::cout << "========== 2D Robot with odometry and 2 LIDARs =============\n";
-
-    unsigned int n_execution = 900; //number of iterations of the whole execution
-
-    // VARIABLES ============================================================================================
-    Eigen::Vector2s odom_data;
-    Eigen::Vector3s ground_truth;
-    Scalar dt = 0.05;
-
-    // Laser params
-    Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
-    laser_1_pose << 1.2, 0, 0, 0; //laser 1
-    laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-    Eigen::VectorXs laser_params(8);
-    laser_params << M_PI/2, -M_PI/2, -M_PI/720, 0.01, 0.2, 100, 0.01, 0.01;
-    std::vector<float> scan1, scan2;
-
-    // Simulated robot
-    FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
-
-    // Initial pose
-    ground_truth << 2, 8, 0;
-
-    //output file
-    std::ofstream laser_1_file, laser_2_file, odom_file, groundtruth_file;
-    groundtruth_file.open("simulated_groundtruth.txt", std::ofstream::out); //open log file
-    odom_file.open("simulated_odom.txt", std::ofstream::out); //open log file
-    laser_1_file.open("simulated_laser_1.txt", std::ofstream::out); //open log file
-    laser_2_file.open("simulated_laser_2.txt", std::ofstream::out); //open log file
-
-    // write laser params
-    laser_1_file << 0 << " " << laser_params.transpose() << " " << robot.myScanner->getNumPoints() << std::endl;
-    laser_2_file << 0 << " " << laser_params.transpose() << " " << robot.myScanner->getNumPoints() << std::endl;
-    laser_1_file << 0 << " " << laser_1_pose.transpose() << std::endl;
-    laser_2_file << 0 << " " << laser_2_pose.transpose() << std::endl;
-
-    // origin frame groundtruth
-    groundtruth_file << 0 << " " << ground_truth.transpose() << std::endl;
-
-    //std::cout << "START TRAJECTORY..." << std::endl;
-    // START TRAJECTORY ============================================================================================
-    for (unsigned int step = 1; step < n_execution; step++)
-    {
-        // ROBOT MOVEMENT ---------------------------
-        ground_truth = robot.motionCampus(step, odom_data(0), odom_data(1));
-
-        // LIDAR DATA ---------------------------
-        scan1 = robot.computeScan(1);
-        scan2 = robot.computeScan(2);
-
-        // writing files ---------------------------
-        groundtruth_file << step*dt << " " << ground_truth.transpose() << std::endl;
-        odom_file << step*dt << " " << odom_data.transpose() << std::endl;
-        laser_1_file << step*dt << " ";
-        for (auto range : scan1)
-            laser_1_file << range << " ";
-        laser_1_file << std::endl;
-        laser_2_file << step*dt << " ";
-        for (auto range : scan1)
-            laser_2_file << range << " ";
-        laser_2_file << std::endl;
-    }
-
-    groundtruth_file.close(); //close log file
-    odom_file.close(); //close log file
-    laser_1_file.close(); //close log file
-    laser_2_file.close(); //close log file
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    //exit
-    return 0;
-}
diff --git a/demos/demo_fcn_ptr.cpp b/demos/demo_fcn_ptr.cpp
deleted file mode 100644
index 8ca82790e934cb0c9236ad83cfe7a92d1cc6d284..0000000000000000000000000000000000000000
--- a/demos/demo_fcn_ptr.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * test_fcn_ptr.cpp
- *
- *  Created on: Nov 29, 2016
- *      Author: jsola
- */
-
-#include <iostream>
-#include <cstdarg>
-
-// define some fcns with differnt # of args
-double half     (double _a)                         { return _a/2;      }
-double quarter  (double _a)                         { return _a/4;      }
-double divide   (double _n, double _d)              { return _n/_d;     }
-double mult     (double _x, double _y)              { return _x*_y;     }
-double mult_div (double _x, double _y, double _z)   { return _x*_y/_z;  }
-
-//======== test_simple usage of function pointers ============
-typedef double (*FcnType)(double);
-
-double run_simple(FcnType f, double a){ return f(a); }
-
-void test_simple()
-{
-    std::cout << "simple..." << std::endl;
-    std::cout << "4/2   = " << run_simple(half,    4) << std::endl;
-    std::cout << "4/4   = " << run_simple(quarter, 4) << std::endl;
-}
-
-//======== more usage of function pointers ============
-typedef double (*FcnType2)(double a, double b);
-typedef double (*FcnType3)(double a, double b, double c);
-
-double run_2(FcnType2 f, double a, double b) { return f(a, b); }
-double run_3(FcnType3 f, double a, double b, double c) { return f(a, b, c); }
-
-void test_more()
-{
-    std::cout << "more..." << std::endl;
-    std::cout << "4/2   = " << run_2(divide,   4, 2)    << std::endl;
-    std::cout << "4*2   = " << run_2(mult,     4, 2)    << std::endl;
-    std::cout << "4*3/6 = " << run_3(mult_div, 4, 3, 6) << std::endl;
-}
-
-//======== variadic usage of function pointers =========
-typedef double (*FcnTypeV)(...);
-
-// we will try to use half(), quarter(), mult(), divide() and mult_div() above
-
-//------------------------------------------------------------------------------------
-// ---- try just to read the args of the variadic fcn; no function pointer yet
-double run_v_dummy(int n, ...)
-{
-    va_list args;
-    va_start(args, n);
-    double b = 0;
-    for (int i=0; i<n; i++)
-        b += va_arg(args, double);
-    return b;
-}
-
-void test_var_dummy()
-{
-    std::cout << "var dummy..." << std::endl;
-    std::cout << "1     = " << run_v_dummy(1, 1.0)           << std::endl;
-    std::cout << "1+2   = " << run_v_dummy(2, 1.0, 2.0)      << std::endl;
-    std::cout << "1+2+3 = " << run_v_dummy(3, 1.0, 2.0, 3.0) << std::endl;
-}
-
-//------------------------------------------------------------------------------------
-// ---- call function through pointer; ugly solution with switch / case on the # of args:
-double run_v_switch(FcnTypeV f, int n, ...)
-{
-    va_list args ;
-    va_start(args, n); // args start after n
-    switch (n)
-    {
-        case 1:
-            return f(va_arg(args, double));
-            break;
-        case 2:
-            return f(va_arg(args, double), va_arg(args, double));
-            break;
-        case 3:
-            return f(va_arg(args, double), va_arg(args, double), va_arg(args, double));
-            break;
-        default:
-            return 0;
-    }
-}
-
-void test_var_switch()
-{
-    std::cout << "var using switch/case of the # of args (ugly)..." << std::endl;
-    std::cout << "4/2   = " << run_v_switch((FcnTypeV)half,     1, 4.0          ) << std::endl;
-    std::cout << "4/4   = " << run_v_switch((FcnTypeV)quarter,  1, 4.0          ) << std::endl;
-    std::cout << "4/2   = " << run_v_switch((FcnTypeV)divide,   2, 4.0, 2.0     ) << std::endl;
-    std::cout << "4*2   = " << run_v_switch((FcnTypeV)mult,     2, 4.0, 2.0     ) << std::endl;
-    std::cout << "4*3/6 = " << run_v_switch((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl;
-}
-
-//------------------------------------------------------------------------------------
-// ---- call function through pointer; try to forward all args straight into the inner function!
-double run_v(FcnTypeV f, int n, ...)
-{
-    va_list args;
-    va_start(args,n);   // args start after n
-    return f(args);     // hop!
-}
-
-void test_var()
-{
-    std::cout << "var forwarding all args to the inner fcn (nice!)..." << std::endl;
-    std::cout << "4/2   = " << run_v((FcnTypeV)half,     1, 4.0          ) << std::endl;
-    std::cout << "4/4   = " << run_v((FcnTypeV)quarter,  1, 4.0          ) << std::endl;
-    std::cout << "4/2   = " << run_v((FcnTypeV)divide,   2, 4.0, 2.0     ) << std::endl;
-    std::cout << "4*2   = " << run_v((FcnTypeV)mult,     2, 4.0, 2.0     ) << std::endl;
-    std::cout << "4*3/6 = " << run_v((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl;
-}
-
-//####################################################################################
-
-int main()
-{
-    test_simple();
-    test_more();
-    test_var_dummy();
-    test_var_switch();
-    test_var();
-}
diff --git a/demos/demo_image.cpp b/demos/demo_image.cpp
deleted file mode 100644
index 3997b9476334d39092ee2eaf5140c5b9a9facfc0..0000000000000000000000000000000000000000
--- a/demos/demo_image.cpp
+++ /dev/null
@@ -1,177 +0,0 @@
-// Testing things for the 3D image odometry
-
-//Wolf includes
-#include "core/sensor/sensor_camera.h"
-#include "core/capture/capture_image.h"
-#include "core/processor/processor_tracker_feature_image.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// Vision utils includes
-#include <vision_utils.h>
-#include <sensors.h>
-#include <common_class/buffer.h>
-#include <common_class/frame.h>
-
-//std includes
-#include <ctime>
-#include <iostream>
-#include <string>
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    //ProcessorImageFeature test
-    std::cout << std::endl << " ========= ProcessorImageFeature test ===========" << std::endl << std::endl;
-
-    // Sensor or sensor recording
-    vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
-    if (sen_ptr==NULL)
-    	return 0;
-
-    unsigned int buffer_size = 8;
-    vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
-    frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
-
-    unsigned int img_width  = frame_buff.back()->getImage().cols;
-    unsigned int img_height = frame_buff.back()->getImage().rows;
-    std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
-
-    // graphics
-    cv::namedWindow("Feature tracker");    // Creates a window for display.
-    cv::moveWindow("Feature tracker", 0, 0);
-    cv::startWindowThread();
-
-    CaptureImagePtr image_ptr;
-
-    TimeStamp t = 1;
-
-    std::string wolf_root = _WOLF_ROOT_DIR;
-    std::cout << "Wolf root: " << wolf_root << std::endl;
-
-    ProblemPtr wolf_problem_ = Problem::create("PO", 3);
-
-    //=====================================================
-    // Method 1: Use data generated here for sensor and processor
-    //=====================================================
-
-//        // SENSOR
-//        Eigen::Vector4s k = {320,240,320,320};
-//        SensorCameraPtr camera_ptr = std::make_shared<SensorCamera>(std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
-//                                                  std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
-//                                                  std::make_shared<StateBlock>(k,false),img_width,img_height);
-//
-//        wolf_problem_->getHardware()->addSensor(camera_ptr);
-//
-//        // PROCESSOR
-//        ProcessorParamsImage tracker_params;
-//        tracker_params.matcher.min_normalized_score = 0.75;
-//        tracker_params.matcher.similarity_norm = cv::NORM_HAMMING;
-//        tracker_params.matcher.roi_width = 30;
-//        tracker_params.matcher.roi_height = 30;
-//        tracker_params.active_search.grid_width = 12;
-//        tracker_params.active_search.grid_height = 8;
-//        tracker_params.active_search.separation = 1;
-//        tracker_params.max_new_features =0;
-//        tracker_params.min_features_for_keyframe = 20;
-//
-//        DetectorDescriptorParamsOrb orb_params;
-//        orb_params.type = DD_ORB;
-//
-//        DetectorDescriptorParamsBrisk brisk_params;
-//        brisk_params.type = DD_BRISK;
-//
-//        // select the kind of detector-descriptor parameters
-//        tracker_params.detector_descriptor_params_ptr = std::make_shared<DetectorDescriptorParamsOrb>(orb_params); // choose ORB
-////        tracker_params.detector_descriptor_params_ptr = std::make_shared<DetectorDescriptorParamsBrisk>(brisk_params); // choose BRISK
-//
-//        std::cout << tracker_params.detector_descriptor_params_ptr->type << std::endl;
-//
-//        ProcessorTrackerTrifocalTensorPtr prc_image = std::make_shared<ProcessorImageFeature>(tracker_params);
-////        camera_ptr->addProcessor(prc_image);
-//        std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-    //=====================================================
-
-    //=====================================================
-    // Method 2: Use factory to create sensor and processor
-    //=====================================================
-
-    /* Do this while there aren't extrinsic parameters on the yaml */
-    Eigen::Vector7s extrinsic_cam;
-    extrinsic_cam[0] = 0; //px
-    extrinsic_cam[1] = 0; //py
-    extrinsic_cam[2] = 0; //pz
-    extrinsic_cam[3] = 0; //qx
-    extrinsic_cam[4] = 0; //qy
-    extrinsic_cam[5] = 0; //qz
-    extrinsic_cam[6] = 1; //qw
-    std::cout << "========extrinsic_cam: " << extrinsic_cam.transpose() << std::endl;
-    const Eigen::VectorXs extr = extrinsic_cam;
-    /* Do this while there aren't extrinsic parameters on the yaml */
-
-    // SENSOR
-    // one-liner API
-    SensorBasePtr sensor_ptr = wolf_problem_->installSensor("CAMERA", "PinHole", Eigen::VectorXs::Zero(7), wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-    SensorCameraPtr camera_ptr = static_pointer_cast<SensorCamera>(sensor_ptr);
-    camera_ptr->setImgWidth(img_width);
-    camera_ptr->setImgHeight(img_height);
-
-    // PROCESSOR
-    // one-liner API
-    ProcessorTrackerFeatureImagePtr prc_img_ptr = std::static_pointer_cast<ProcessorTrackerFeatureImage>( wolf_problem_->installProcessor("IMAGE FEATURE", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml") );
-    std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-    //=====================================================
-
-//    // 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.minimizer_progress_to_stdout = false;
-//    // ceres_options.line_search_direction_type = ceres::LBFGS;
-//    // ceres_options.max_num_iterations = 100;
-//    google::InitGoogleLogging(argv[0]);
-//    CeresManager ceres_manager(wolf_problem_, ceres_options);
-
-    for(int f = 0; f<10000; ++f)
-    {
-        frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), f) );
-
-    	std::cout << "\n=============== Frame #: " << f << " ===============" << std::endl;
-
-        t.setToNow();
-        clock_t t1 = clock();
-
-        // Preferred method with factory objects:
-        image_ptr = make_shared<CaptureImage>(t, camera_ptr, frame_buff.back()->getImage());
-
-        camera_ptr->process(image_ptr);
-
-        std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
-
-        wolf_problem_->print();
-
-        cv::Mat image = frame_buff.back()->getImage().clone();
-        prc_img_ptr->drawFeatures(image);
-        prc_img_ptr->drawRoi(image,prc_img_ptr->detector_roi_,cv::Scalar(0.0,255.0, 255.0));   //active search roi
-        prc_img_ptr->drawRoi(image,prc_img_ptr->tracker_roi_, cv::Scalar(255.0, 0.0, 255.0));  //tracker roi
-        prc_img_ptr->drawTarget(image,prc_img_ptr->tracker_target_);
-        cv::imshow("Feature tracker", image);
-        cv::waitKey(1);
-
-//        if((f%100) == 0)
-//        {
-//            std::string summary = ceres_manager.solve(2);
-//            std::cout << summary << std::endl;
-//
-//            std::cout << "Last key frame pose: "
-//                      << wolf_problem_->getLastKeyFrame()->getP()->getState().transpose() << std::endl;
-//            std::cout << "Last key frame orientation: "
-//                      << wolf_problem_->getLastKeyFrame()->getO()->getState().transpose() << std::endl;
-//
-//            cv::waitKey(0);
-//        }
-    }
-}
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
deleted file mode 100644
index c05fbbf9c4e676c5611c7073b94b9f55385a9267..0000000000000000000000000000000000000000
--- a/demos/demo_imuDock.cpp
+++ /dev/null
@@ -1,318 +0,0 @@
-/**
- * \file test_imuDock.cpp
- *
- *  Created on: July 18, 2017
- *      \author: Dinesh Atchuthan
- */
-
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-
-//Factors headers
-#include "core/factor/factor_fix_bias.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include "core/factor/factor_pose_3D.h"
-
-#define OUTPUT_RESULTS
-//#define ADD_KF3
-
-/*                              OFFLINE VERSION
-    In this test, we use the experimental conditions needed for Humanoids 2017.
-    IMU data are acquired using the docking station. 
-
-    Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
-    invar       : P1, V1, V2
-    var         : Q1,B1,P2,Q2,B2
-    factors : Odometry factor between KeyFrames
-                  IMU factor
-                  FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
-                  Fix3D factor
-
-    What we expect  : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
-                      Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
-
-    Representation of the application:
-
-                                    Imu
-                        KF1----------â—¼----------KF2
-                   /----P1----------\ /----------P2             invar       : P1, V1, V2
-        Abs|------â—¼                 â—¼                          var         : Q1,B1,P2,Q2,B2
-                   \----Q1----------/ \----------Q2
-                        V1          Odom
-        Abs|------â—¼-----B1
-*/
-int main(int argc, char** argv)
-{
-    //#################################################### INITIALIZATION
-    using namespace wolf;
-
-    //___get input file for imu data___
-    std::ifstream imu_data_input;
-    const char * filename_imu;
-    if (argc < 02)
-    {
-        WOLF_ERROR("Missing 1 input argument (path to imu data file).")
-        return 1; //return with error
-    }
-    else
-    {
-        filename_imu = argv[1];
-
-        imu_data_input.open(filename_imu);
-        WOLF_INFO("imu file : ", filename_imu)
-    }
-
-    // ___Check if the file is correctly opened___
-    if(!imu_data_input.is_open()){
-        WOLF_ERROR("Failed to open data file ! Exiting")
-        return 1;
-    }
-
-    #ifdef OUTPUT_RESULTS
-        //define output file
-        std::ofstream output_results_before, output_results_after, checking;
-        output_results_before.open("imu_dock_beforeOptim.dat");
-        output_results_after.open("imu_dock_afterOptim.dat");
-        checking.open("KF_pose_stdev.dat");
-    #endif
-
-    // ___initialize variabes that will be used through the code___
-    Eigen::VectorXs problem_origin(16);
-    Eigen::Vector7s imu_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished());
-    problem_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0;
-    
-    //Create vectors to store data and time
-    Eigen::Vector6s data_imu, data_odom;
-    Scalar clock;
-    TimeStamp ts(0), ts_output(0); //will be used to store the data timestamps and set timestamps in captures
-
-    // ___Define expected values___
-    Eigen::Vector7s expected_KF1_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7s()<<0,-0.06,0,0,0,0,11).finished());
-
-    //#################################################### SETTING PROBLEM
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    // ___Create the WOLF Problem + define origin of the problem___
-    ProblemPtr problem = Problem::create("PQVBB 3D");
-    CeresManager* ceres_manager = new CeresManager(problem);
- 
-    // ___Configure Ceres if needed___
-
-    // ___Create sensors + processors___
-    SensorIMUPtr sensorIMU = std::static_pointer_cast<SensorIMU>(problem->installSensor("IMU", "Main IMU", imu_pose, wolf_root + "/src/examples/sensor_imu.yaml"));
-    ProcessorIMUPtr processorIMU = std::static_pointer_cast<ProcessorIMU>(problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"));
-    
-    SensorOdom3DPtr sensorOdom = std::static_pointer_cast<SensorOdom3D>(problem->installSensor("ODOM 3D", "odom", odom_pose, wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"));
-    ProcessorOdom3DPtr processorOdom = std::static_pointer_cast<ProcessorOdom3D>(problem->installProcessor("ODOM 3D", "odom", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml"));
-    
-    // ___set origin of processors to the problem's origin___
-    FrameIMUPtr KF1 = std::static_pointer_cast<FrameIMU>(processorIMU->setOrigin(problem_origin, ts)); // XXX JS: setting ts to zero, and then reading clock from data, is inconsistent.
-    processorOdom->setOrigin(KF1);
-
-    //#################################################### PROCESS DATA
-    // ___process IMU and odometry___
-
-    //Create captures
-    CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6s::Identity(), Vector6s::Zero()); //ts is set at 0
-    CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(ts, sensorOdom, data_odom, 7, 6, nullptr);
-
-    //while we do not reach the end of file, read IMU input (ts, Ax, Ay, Az, Wx, Wy, Wz) and process data through capture
-    while(!imu_data_input.eof())
-    {
-        //read
-        imu_data_input >> clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5];
-
-        //set capture
-        ts.set(clock);
-        imu_ptr->setTimeStamp(ts);
-        imu_ptr->setData(data_imu);
-
-        //process
-        sensorIMU->process(imu_ptr);
-    }
-
-    //All IMU data have been processed, close the file
-    imu_data_input.close();
-
-    //A KeyFrame should have been created (depending on time_span in processors). get the last KeyFrame
-    // XXX JS: in my  opinion, we should control the KF creation better, not using time span. Is it possible?
-    #ifdef ADD_KF3
-        //Add a KeyFrame just before the motion actually starts (we did not move yet)
-        data_odom << 0,0,0, 0,0,0;
-        TimeStamp t_middle(0.307585);
-        mot_ptr->setTimeStamp(t_middle);
-        mot_ptr->setData(data_odom);
-        sensorOdom->process(mot_ptr);
-
-        //Also add a keyframe at the end of the motion
-        data_odom << 0,-0.06,0, 0,0,0;
-        ts.set(0.981573); //comment this if you want the last KF to be created at last imu's ts
-        mot_ptr->setTimeStamp(ts);
-        mot_ptr->setData(data_odom);
-        sensorOdom->process(mot_ptr);
-
-        FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(t_middle));
-        FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-    #else
-        //now impose final odometry using last timestamp of imu
-        data_odom << 0,-0.06,0, 0,0,0;
-        mot_ptr->setTimeStamp(ts);
-        mot_ptr->setData(data_odom);
-        sensorOdom->process(mot_ptr);
-
-        FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-    #endif
-
-    //#################################################### OPTIMIZATION PART
-    // ___Create needed factors___
-
-    //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
-    Eigen::MatrixXs featureFix_cov(6,6);
-    featureFix_cov = Eigen::MatrixXs::Identity(6,6);
-    featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
-    featureFix_cov(3,3) = pow( .02  , 2); // roll variance
-    featureFix_cov(4,4) = pow( .02  , 2); // pitch variance
-    featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
-    CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
-    FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
-    FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
-
-    Eigen::MatrixXs featureFixBias_cov(6,6);
-    featureFixBias_cov = Eigen::MatrixXs::Identity(6,6); 
-    featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
-    featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
-    CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
-    //create a FeatureBase to factor biases
-    FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
-    FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
-
-    // ___Fix/Unfix stateblocks___
-    KF1->getP()->fix();
-    KF1->getO()->unfix();
-    KF1->getV()->fix();
-    KF1->getAccBias()->unfix();
-    KF1->getGyroBias()->unfix();
-
-    #ifdef ADD_KF3
-        KF2->getP()->fix();
-        KF2->getO()->unfix();
-        KF2->getV()->fix();
-        KF2->getAccBias()->unfix();
-        KF2->getGyroBias()->unfix();
-
-        KF3->getP()->unfix();
-        KF3->getO()->unfix();
-        KF3->getV()->fix();
-        KF3->getAccBias()->unfix();
-        KF3->getGyroBias()->unfix();
-    #else
-        KF2->getP()->unfix();
-        KF2->getO()->unfix();
-        KF2->getV()->fix();
-        KF2->getAccBias()->unfix();
-        KF2->getGyroBias()->unfix();
-    #endif
-
-    #ifdef OUTPUT_RESULTS
-        // ___OUTPUT___
-        /* Produce output file for matlab visualization
-         * first output : estimated trajectory BEFORE optimization (getting the states each millisecond)
-         */
-
-        unsigned int time_iter(0);
-        Scalar ms(0.001);
-        ts_output.set(0);
-        while(ts_output.get() < ts.get() + ms)
-        {
-            output_results_before << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
-            time_iter++;
-            ts_output.set(time_iter * ms);
-        }
-    #endif
-
-    // ___Solve + compute covariances___
-    problem->print(4,0,1,0);
-    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    problem->print(1,0,1,0);
-
-    //#################################################### RESULTS PART
-
-    // ___Get standard deviation from covariances___
-    #ifdef ADD_KF3
-        Eigen::MatrixXs cov_KF1(16,16), cov_KF2(16,16), cov_KF3(16,16);
-
-        problem->getFrameCovariance(KF1, cov_KF1);
-        problem->getFrameCovariance(KF2, cov_KF2);
-        problem->getFrameCovariance(KF3, cov_KF3);
-
-        Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF1, stdev_KF2, stdev_KF3;
-
-        stdev_KF1 = cov_KF1.diagonal().array().sqrt();
-        stdev_KF2 = cov_KF2.diagonal().array().sqrt();
-        stdev_KF3 = cov_KF3.diagonal().array().sqrt();
-
-        WOLF_DEBUG("stdev KF1 : ", stdev_KF1.transpose());
-        WOLF_DEBUG("stdev KF2 : ", stdev_KF2.transpose());
-        WOLF_DEBUG("stdev KF3 : ", stdev_KF3.transpose());
-    #else
-        Eigen::MatrixXs cov_KF1(16,16), cov_KF2(16,16);
-
-        problem->getFrameCovariance(KF1, cov_KF1);
-        problem->getFrameCovariance(KF2, cov_KF2);
-
-        Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF1, stdev_KF2;
-
-        stdev_KF1 = cov_KF1.diagonal().array().sqrt();
-        stdev_KF2 = cov_KF2.diagonal().array().sqrt();
-
-        WOLF_DEBUG("stdev KF1 : \n", stdev_KF1.transpose());
-        WOLF_DEBUG("stdev KF2 : \n", stdev_KF2.transpose());
-    #endif
-    
-
-    #ifdef OUTPUT_RESULTS
-        // ___OUTPUT___
-        /* Produce output file for matlab visualization
-         * Second output:   KF2 position standard deviation computed
-         *                  estimated trajectory AFTER optimization 
-         *                  + get KF2 timestamp + state just in case the loop is not working as expected
-         */
-
-        //estimated trajectort
-        time_iter = 0;
-        ts_output.set(0);
-        while(ts_output.get() < ts.get() + ms)
-        {
-            output_results_after << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
-            time_iter++;
-            ts_output.set(time_iter * ms);
-        }
-
-        //finally, output the timestamp, state and stdev associated to KFs
-        #ifdef ADD_KF3
-            checking << KF2->getTimeStamp().get() << "\t" << KF2->getState().transpose() << "\t" << stdev_KF2.transpose() << std::endl;
-            checking << KF3->getTimeStamp().get() << "\t" << KF3->getState().transpose() << "\t" << stdev_KF3.transpose() << std::endl;
-        #else
-            checking << KF2->getTimeStamp().get() << "\t" << KF2->getState().transpose() << "\t" << stdev_KF2.transpose() << std::endl;
-        #endif
-    #endif
-    
-    // ___Are expected values in the range of estimated +/- 2*stdev ?___
-
-    #ifdef OUTPUT_RESULTS
-        output_results_before.close();
-        output_results_after.close();
-        checking.close();
-    #endif
-
-    return 0;
-}
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
deleted file mode 100644
index 039615445807ab6ef9e1dadab7e273135bc650ef..0000000000000000000000000000000000000000
--- a/demos/demo_imuDock_autoKFs.cpp
+++ /dev/null
@@ -1,311 +0,0 @@
-/**
- * \file test_imuDock_autoKFs.cpp
- *
- *  Created on: July 22, 2017
- *      \author: Dinesh Atchuthan
- */
-
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-
-//Factors headers
-#include "core/factor/factor_fix_bias.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include "core/factor/factor_pose_3D.h"
-
-#define OUTPUT_RESULTS
-//#define AUTO_KFS
-
-/*                              OFFLINE VERSION
-    In this test, we use the experimental conditions needed for Humanoids 2017.
-    IMU data are acquired using the docking station. 
-
-    Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
-    invar       : P1, V1, V2
-    var         : Q1,B1,P2,Q2,B2
-
-    All Keyframes coming after KF2 are constrained just like KF2
-    factors : Odometry factor between KeyFrames
-                  IMU factor
-                  FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
-                  Fix3D factor
-
-    What we expect  : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
-                      Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
-
-    Representation of the application:
-
-                                    Imu
-                        KF1----------â—¼----------KF2--..
-                   /----P1----------\ /----------P2--..             invar       : P1, V1, V2
-        Abs|------â—¼                 â—¼                          var         : Q1,B1,P2,Q2,B2
-                   \----Q1----------/ \----------Q2--..
-                        V1          Odom         v2  ..
-        Abs|------â—¼-----B1                       B2  ..
-*/
-int main(int argc, char** argv)
-{
-    //#################################################### INITIALIZATION
-    using namespace wolf;
-
-    //___get input files for imu and odom data___
-    std::ifstream imu_data_input, odom_data_input;
-    const char * filename_imu;
-    const char * filename_odom;
-    if (argc < 03)
-    {
-        WOLF_ERROR("Missing input argument : path to imu or/and odom data file(s).")
-        return 1; //return with error
-    }
-    else
-    {
-        filename_imu = argv[1];
-        filename_odom = argv[2];
-
-        imu_data_input.open(filename_imu);
-        WOLF_INFO("imu file : ", filename_imu)
-        odom_data_input.open(filename_odom);
-        WOLF_INFO("odom file : ", filename_odom)
-    }
-
-    // ___Check if the file is correctly opened___
-    if(!imu_data_input.is_open() || !odom_data_input.is_open()){
-        WOLF_ERROR("Failed to open data file ! Exiting")
-        return 1;
-    }
-
-    #ifdef OUTPUT_RESULTS
-        //define output file
-        std::ofstream output_results_before, output_results_after, checking;
-        output_results_before.open("imu_dock_beforeOptim.dat");
-        output_results_after.open("imu_dock_afterOptim.dat");
-        checking.open("KF_pose_stdev.dat");
-    #endif
-
-    // ___initialize variabes that will be used through the code___
-    Eigen::VectorXs problem_origin(16);
-    Eigen::Vector7s imu_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished());
-    problem_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0;
-    
-    //Create vectors to store data and time
-    Eigen::Vector6s data_imu, data_odom;
-    Scalar clock;
-    TimeStamp ts(0), ts_output(0); //will be used to store the data timestamps and set timestamps in captures
-
-    // ___Define expected values___
-    Eigen::Vector7s expected_KF1_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7s()<<0,-0.06,0,0,0,0,11).finished());
-
-    #ifdef AUTO_KFs
-        std::array<Scalar, 50> lastms_imuData;
-    #endif
-    //#################################################### SETTING PROBLEM
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    // ___Create the WOLF Problem + define origin of the problem___
-    ProblemPtr problem = Problem::create("PQVBB 3D");
-    CeresManager* ceres_manager = new CeresManager(problem);
- 
-    // ___Configure Ceres if needed___
-
-    // ___Create sensors + processors___
-    SensorIMUPtr sensorIMU = std::static_pointer_cast<SensorIMU>(problem->installSensor("IMU", "Main IMU", imu_pose, wolf_root + "/src/examples/sensor_imu.yaml"));
-    ProcessorIMUPtr processorIMU = std::static_pointer_cast<ProcessorIMU>(problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"));
-    
-    SensorOdom3DPtr sensorOdom = std::static_pointer_cast<SensorOdom3D>(problem->installSensor("ODOM 3D", "odom", odom_pose, wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"));
-    ProcessorOdom3DPtr processorOdom = std::static_pointer_cast<ProcessorOdom3D>(problem->installProcessor("ODOM 3D", "odom", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml"));
-    
-    // ___set origin of processors to the problem's origin___
-    FrameIMUPtr KF1 = std::static_pointer_cast<FrameIMU>(processorIMU->setOrigin(problem_origin, ts)); // XXX JS: setting ts to zero, and then reading clock from data, is inconsistent.
-    processorOdom->setOrigin(KF1);
-
-    //#################################################### PROCESS DATA
-    // ___process IMU and odometry___
-
-    //Create captures
-    CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6s::Identity(), Vector6s::Zero()); //ts is set at 0
-    CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(ts, sensorOdom, data_odom, 7, 6, nullptr);
-
-    //while we do not reach the end of file, read IMU input (ts, Ax, Ay, Az, Wx, Wy, Wz) and process data through capture
-    while(!imu_data_input.eof())
-    {
-        //read
-        imu_data_input >> clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5];
-
-        //set capture
-        ts.set(clock);
-        imu_ptr->setTimeStamp(ts);
-        imu_ptr->setData(data_imu);
-
-        //process
-        sensorIMU->process(imu_ptr);
-    }
-
-    //Process all the odom data
-    // XXX JS: in my  opinion, we should control the KF creation better, not using time span. Is it possible?
-    while(!odom_data_input.eof())
-    {
-        //read
-        odom_data_input >> clock >> data_odom[0] >> data_odom[1] >> data_odom[2] >> data_odom[3] >> data_odom[4] >> data_odom[5];
-
-        //set capture
-        ts.set(clock);
-        mot_ptr->setTimeStamp(ts);
-        mot_ptr->setData(data_odom);
-
-        #ifdef AUTO_KFS
-            /* We want the KFs to be generated automatically but not using time span as argument of this generation
-             * For our application, w want the KFs to be generated when an odometry data is given under condition that the IMU is not moving
-             * We check wether the IMU is moving or not by computing the current stdev of the IMU based on data received during 50ms before the odom timestamp
-             * We compare this value to the stdev (noise) of the sensor (see sensor_imu.yaml)
-             * If the current stdev is below a threshold then we process the odometry data !
-             */
-
-             // TODO : get data to compute stdev with directly from the capture
-             //         -> see how these data are stored and change getIMUStdev(..) function defined below main() in this file
-             //         -> then just use the function to get this stdev of corresponding data
-
-        #else
-            //process anyway. KFs will be generated based on the configuration given in processor_odom_3D.yaml
-            sensorOdom->process(mot_ptr);
-        #endif
-    }
-
-    //All data have been processed, close the files
-    imu_data_input.close();
-    odom_data_input.close();
-    
-    //A KeyFrame should have been created (depending on time_span in processors). get all frames
-    FrameBasePtrList trajectory = problem->getTrajectory()->getFrameList();
-    
-
-    //#################################################### OPTIMIZATION PART
-    // ___Create needed factors___
-
-    //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
-    Eigen::MatrixXs featureFix_cov(6,6);
-    featureFix_cov = Eigen::MatrixXs::Identity(6,6);
-    featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
-    featureFix_cov(3,3) = pow( .01  , 2); // roll variance
-    featureFix_cov(4,4) = pow( .01  , 2); // pitch variance
-    featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
-    CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
-    FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
-    FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
-
-    Eigen::MatrixXs featureFixBias_cov(6,6);
-    featureFixBias_cov = Eigen::MatrixXs::Identity(6,6); 
-    featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
-    featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
-    CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
-    //create a FeatureBase to factor biases
-    FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
-    FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
-
-    // ___Fix/Unfix stateblocks___
-    // fix all Keyframes here
-
-    FrameIMUPtr frame_imu;
-    for(auto frame : trajectory)
-    {   
-        frame_imu = std::static_pointer_cast<FrameIMU>(frame);
-        if(frame_imu->isKey())
-        {
-            frame_imu->getP()->fix();
-            frame_imu->getO()->unfix();
-            frame_imu->getV()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
-            frame_imu->getV()->fix();
-            frame_imu->getAccBias()->unfix();
-            frame_imu->getGyroBias()->unfix();
-        }
-    }
-    
-    //KF1 (origin) needs to be also fixed in position
-    KF1->getP()->fix();
-
-    #ifdef OUTPUT_RESULTS
-        // ___OUTPUT___
-        /* Produce output file for matlab visualization
-         * first output : estimated trajectory BEFORE optimization (getting the states each millisecond)
-         */
-
-        unsigned int time_iter(0);
-        Scalar ms(0.001);
-        ts_output.set(0);
-        while(ts_output.get() < ts.get() + ms)
-        {
-            output_results_before << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
-            time_iter++;
-            ts_output.set(time_iter * ms);
-        }
-    #endif
-
-    // ___Solve + compute covariances___
-    problem->print(4,0,1,0);
-    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-    ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    problem->print(1,0,1,0);
-
-    //#################################################### RESULTS PART
-
-    // ___Get standard deviation from covariances___ and output this in a file
-    Eigen::MatrixXs cov_KF(16,16);
-    Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF;
-    for(auto frame : trajectory)
-    {
-        if(frame->isKey())
-        {
-            problem->getFrameCovariance(frame, cov_KF);
-            stdev_KF = cov_KF.diagonal().array().sqrt();
-            #ifdef OUTPUT_RESULTS
-                checking << frame->getTimeStamp().get() << "\t" << frame->getState().transpose() << "\t" << stdev_KF.transpose() << std::endl;
-            #endif
-        }
-    }
-
-    #ifdef OUTPUT_RESULTS
-        // ___OUTPUT___
-        /* Produce output file for matlab visualization
-         * Second output:   KF position standard deviation computed
-         *                  estimated trajectory AFTER optimization 
-         */
-
-        //estimated trajectort
-        time_iter = 0;
-        ts_output.set(0);
-        while(ts_output.get() < ts.get() + ms)
-        {
-            output_results_after << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
-            time_iter++;
-            ts_output.set(time_iter * ms);
-        }
-
-        //Finished writing in files : close them
-        output_results_before.close();
-        output_results_after.close();
-        checking.close();
-    #endif
-
-    return 0;
-}
-
-/*Scalar getIMUStdev(Eigen::VectorXs _data) //input argument : whatever will contain the data in the capture
-{
-    Eigen::Vector6s mean(Eigen::Vector6s::Zero()), stdev(Eigen::Vector6s::Zero());
-    unsigned int _data_size(_data.size());
-    
-    mean = _data.mean()/_data_size;
-
-    for (unsigned int i = 0; i < _data_size; i++)
-    {
-        stdev += pow(_data()-mean,2); //get the correct data from the container
-    }
-    return (stdev.array().sqrt()).maxCoeff();
-}*/
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
deleted file mode 100644
index e0e4e4778e91090221d70121bd89907ad3756823..0000000000000000000000000000000000000000
--- a/demos/demo_imuPlateform_Offline.cpp
+++ /dev/null
@@ -1,266 +0,0 @@
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-#define DEBUG_RESULTS
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    // LOADING DATA FILE (IMU)
-    // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
-    
-    std::ifstream imu_data_input;
-    const char * filename_imu;
-    if (argc < 02)
-    {
-        
-        WOLF_ERROR("Missing input argument! : needs 1 argument (path to imu data file).")
-        return 1;
-    }
-    else
-    {
-        filename_imu = argv[1];
-
-        imu_data_input.open(filename_imu);
-        WOLF_INFO("imu file : ", filename_imu)
-
-        //std::string dummy; //this is needed only if first line is headers or useless data
-        //getline(imu_data_input, dummy);
-    }
-
-    if(!imu_data_input.is_open()){
-        std::cerr << "Failed to open data files... Exiting" << std::endl;
-        return 1;
-    }
-
-    #ifdef DEBUG_RESULTS
-    std::ofstream debug_results;
-    debug_results.open("debug_results_imu_constrained0.dat");
-    if(debug_results)
-        debug_results   << "%%TimeStamp\t"
-                        << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
-                        << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
-    #endif
-
-    //===================================================== SETTING PROBLEM
-    std::string wolf_root = _WOLF_ROOT_DIR;
-        
-    // WOLF PROBLEM
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
-    Eigen::VectorXs x_origin(16);
-    x_origin << 0,0,0,  0,0,0,1,  0,0,0,  0,0,0,  0,0,0; //INITIAL CONDITIONS
-    TimeStamp t(0);
-
-    // CERES WRAPPER
-    ceres::Solver::Options ceres_options;
-    ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-    ceres_options.max_line_search_step_contraction = 1e-3;
-    ceres_options.max_num_iterations = 1e4;
-    CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
-    // SENSOR + PROCESSOR IMU
-    SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-    ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
-    prc_imu_params->max_time_span = 10;
-    prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-    prc_imu_params->dist_traveled = 1000000000;
-    prc_imu_params->angle_turned = 1000000000;
-    
-    ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", sen0_ptr, prc_imu_params);
-    SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-    ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
-    // SENSOR + PROCESSOR ODOM 3D
-    SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
-    ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
-    prc_odom3D_params->max_time_span = 1.9999;
-    prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-    prc_odom3D_params->dist_traveled = 1000000000;
-    prc_odom3D_params->angle_turned = 1000000000;
-
-    ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
-    SensorOdom3DPtr sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
-    ProcessorOdom3DPtr processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
-    // reset origin of problem
-    t.set(0);
-    
-    FrameIMUPtr origin_KF = std::static_pointer_cast<FrameIMU>(processor_ptr_imu->setOrigin(x_origin, t));
-    processor_ptr_odom3D->setOrigin(origin_KF);
-
-    //fix parts of the problem if needed
-    origin_KF->getP()->fix();
-    origin_KF->getO()->fix();
-    //===================================================== PROCESS DATA
-    // PROCESS DATA
-
-    Eigen::Vector6s data_imu, data_odom3D;
-    data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
-    data_odom3D << 0,-0.06,0, 0,0,0;
-
-    Scalar input_clock;
-    TimeStamp ts(0);
-    wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6s::Identity(), Vector6s::Zero());
-    wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 6, 6, nullptr);
-
-    //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
-    t = ts;
-
-    clock_t begin = clock();
-    
-    while( !imu_data_input.eof())
-    {
-        // PROCESS IMU DATA
-        // Time and data variables
-        imu_data_input >> input_clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5]; //Ax, Ay, Az, Gx, Gy, Gz
-
-        ts.set(input_clock);
-        imu_ptr->setTimeStamp(ts);
-        imu_ptr->setData(data_imu);
-
-        // process data in capture
-        imu_ptr->getTimeStamp();
-        sen_imu->process(imu_ptr);
-    }
-    
-    TimeStamp t0, tf;
-    t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
-    tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-
-    //Finally, process the only one odom input
-    mot_ptr->setTimeStamp(ts);
-    mot_ptr->setData(data_odom3D);
-    sen_odom3D->process(mot_ptr);
-
-    clock_t end = clock();
-    FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-
-    //closing file
-    imu_data_input.close();
-    //===================================================== END{PROCESS DATA}
-
-    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
-    // Final state
-    std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
-    std::cout << "Initial    state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << x_origin.head(16).transpose() << std::endl;
-    std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
-    std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
-    std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
-    << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
-    // Print statistics
-    std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
-    std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-    /*TimeStamp t0, tf;
-    t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
-    tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();*/
-    std::cout << "t0        : " << t0.get() << " s" << std::endl;
-    std::cout << "tf        : " << tf.get() << " s" << std::endl;
-    std::cout << "duration  : " << tf-t0 << " s" << std::endl;
-    std::cout << "N samples : " << N << std::endl;
-    std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
-    std::cout << "CPU time  : " << elapsed_secs << " s" << std::endl;
-    std::cout << "s/integr  : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
-    std::cout << "integr/s  : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
-    //fix parts of the problem if needed
-    origin_KF->getP()->fix();
-    origin_KF->getO()->fix();
-    origin_KF->getV()->fix();
-    
-    std::cout << "\t\t\t ______solving______" << std::endl;
-    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
-    std::cout << report << std::endl;
-    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
-    std::cout << "\t\t\t ______solved______" << std::endl;
-
-    wolf_problem_ptr_->print(4,1,1,1);
-
-    #ifdef DEBUG_RESULTS
-    Eigen::VectorXs frm_state(16);
-    Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev;
-    Eigen::MatrixXs covX(16,16);
-    Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
-
-    wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
-    for(FrameBasePtr frm_ptr : frame_list)
-    {
-        if(frm_ptr->isKey())
-        {   
-            //prepare needed variables
-            FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
-            frm_state = frmIMU_ptr->getState();
-            ts = frmIMU_ptr->getTimeStamp();
-
-            //get data from covariance blocks
-            wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
-            wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
-            covX.block(7,7,3,3) = cov3;
-            wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
-            covX.block(10,10,3,3) = cov3;
-            wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
-            covX.block(13,13,3,3) = cov3;
-            for(int i = 0; i<16; i++)
-                cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
-            debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
-            << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
-            << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
-            << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15)
-            << "\t" << cov_stdev(0) << "\t" << cov_stdev(1) << "\t" << cov_stdev(2)
-            << "\t" << cov_stdev(3) << "\t" << cov_stdev(4) << "\t" << cov_stdev(5) << "\t" << cov_stdev(6)
-            << "\t" << cov_stdev(7) << "\t" << cov_stdev(8) << "\t" << cov_stdev(9)
-            << "\t" << cov_stdev(10) << "\t" << cov_stdev(11) << "\t" << cov_stdev(12) << "\t" << cov_stdev(13) << "\t" << cov_stdev(14) << "\t" << cov_stdev(15) << std::endl;
-        }
-    }
-
-    debug_results.close();
-    WOLF_WARN("WARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)")
-
-    #endif
-
-    return 0;
-
-}
-
-int _kbhit()
-{
-    struct timeval tv;
-    fd_set fds;
-    tv.tv_sec = 0;
-    tv.tv_usec = 0;
-    FD_ZERO(&fds);
-    FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
-    select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
-    return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
deleted file mode 100644
index 817b08a14a4c71caf9f9e4807c71cc14cc43d78e..0000000000000000000000000000000000000000
--- a/demos/demo_imu_constrained0.cpp
+++ /dev/null
@@ -1,374 +0,0 @@
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-#define DEBUG_RESULTS
-#define KF0_EVOLUTION
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    // LOADING DATA FILES (IMU + ODOM)
-    // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
-    // FOR ODOM, file content is : Timestampt\t Δpx\t  Δpy\t  Δpz\t  Δox\t  Δoy\t  Δoz
-    
-    std::ifstream imu_data_input;
-    std::ifstream odom_data_input;
-    const char * filename_imu;
-    const char * filename_odom;
-    if (argc < 3)
-    {
-        std::cout << "Missing input argument! : needs 2 argument (path to imu and odom data files)." << std::endl;
-        return 1;
-    }
-    else
-    {
-        filename_imu = argv[1];
-        filename_odom = argv[2];
-
-        imu_data_input.open(filename_imu);
-        odom_data_input.open(filename_odom);
-
-        std::cout << "file imu : " << filename_imu <<"\t file odom : " << filename_odom << std::endl;
-
-        //std::string dummy; //this is needed only if first line is headers or useless data
-        //getline(imu_data_input, dummy);
-    }
-
-    if(!imu_data_input.is_open() || !odom_data_input.is_open()){
-        std::cerr << "Failed to open data files... Exiting" << std::endl;
-        return 1;
-    }
-
-    #ifdef DEBUG_RESULTS
-    std::ofstream debug_results;
-    debug_results.open("debug_results_imu_constrained0.dat");
-    if(debug_results)
-        debug_results   << "%%TimeStamp\t"
-                        << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
-                        << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
-    #endif
-
-    #ifdef KF0_EVOLUTION
-    std::ofstream KF0_evolution;
-    KF0_evolution.open("KF0_evolution.dat");
-    if(KF0_evolution)
-        KF0_evolution   << "%%TimeStamp\t"
-                        << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
-                        << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
-    #endif
-
-    //===================================================== SETTING PROBLEM
-    std::string wolf_root = _WOLF_ROOT_DIR;
-        
-    // WOLF PROBLEM
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
-    Eigen::VectorXs x_origin(16);
-    Eigen::Vector6s origin_bias;
-    x_origin << 0,0,0,  0,0,0,1,  0,0,0,  0,0,0,  0,0,0; //INITIAL CONDITIONS    0.05,0.03,.00,  0.2,-0.05,.00;
-    TimeStamp t(0);
-
-    // initial conditions defined from data file
-    // remember that matlab's quaternion is W,X,Y,Z and the one in Eigen has X,Y,Z,W form
-    imu_data_input >> x_origin[0] >> x_origin[1] >> x_origin[2] >> x_origin[6] >> x_origin[3] >> x_origin[4] >> x_origin[5] >> x_origin[7] >> x_origin[8] >> x_origin[9];
-    imu_data_input >> origin_bias[0] >> origin_bias[1] >> origin_bias[2] >> origin_bias[3] >> origin_bias[4] >> origin_bias[5];
-
-    // CERES WRAPPER
-    ceres::Solver::Options ceres_options;
-    ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-    ceres_options.max_line_search_step_contraction = 1e-3;
-    ceres_options.max_num_iterations = 1e4;
-    CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
-    // SENSOR + PROCESSOR IMU
-    SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-    ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
-    prc_imu_params->max_time_span = 10;
-    prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-    prc_imu_params->dist_traveled = 1000000000;
-    prc_imu_params->angle_turned = 1000000000;
-    
-    ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", sen0_ptr, prc_imu_params);
-    SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-    ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
-    // SENSOR + PROCESSOR ODOM 3D
-    SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
-    ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
-    prc_odom3D_params->max_time_span = 20.9999;
-    prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-    prc_odom3D_params->dist_traveled = 1000000000;
-    prc_odom3D_params->angle_turned = 1000000000;
-
-    ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
-    SensorOdom3DPtr sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
-    ProcessorOdom3DPtr processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
-    // reset origin of problem
-    t.set(0);
-    Eigen::Matrix<wolf::Scalar, 10, 1> expected_final_state;
-    
-    FrameIMUPtr origin_KF = std::static_pointer_cast<FrameIMU>(processor_ptr_imu->setOrigin(x_origin, t));
-    processor_ptr_odom3D->setOrigin(origin_KF);
-
-    odom_data_input >> expected_final_state[0] >> expected_final_state[1] >> expected_final_state[2] >> expected_final_state[6] >> expected_final_state[3] >>
-                            expected_final_state[4] >> expected_final_state[5] >> expected_final_state[7] >> expected_final_state[8] >> expected_final_state[9];
-
-    //fix parts of the problem if needed
-    origin_KF->getP()->fix();
-    origin_KF->getO()->fix();
-    //===================================================== PROCESS DATA
-    // PROCESS DATA
-
-    Eigen::Vector6s data_imu, data_odom3D;
-    data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
-    data_odom3D << 0,0,0, 0,0,0;
-
-    Scalar input_clock;
-    TimeStamp ts(0);
-    TimeStamp t_odom(0);
-    wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6s::Identity(), Vector6s::Zero());
-    wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 6, 6, nullptr);
-    
-    //read first odom data from file
-    odom_data_input >> input_clock >> data_odom3D[0] >> data_odom3D[1] >> data_odom3D[2] >> data_odom3D[3] >> data_odom3D[4] >> data_odom3D[5];
-    t_odom.set(input_clock);
-    //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
-    t = ts;
-
-    clock_t begin = clock();
-    
-    while( !imu_data_input.eof() && !odom_data_input.eof() )
-    {
-        // PROCESS IMU DATA
-        // Time and data variables
-        imu_data_input >> input_clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5]; //Ax, Ay, Az, Gx, Gy, Gz
-
-        ts.set(input_clock);
-        imu_ptr->setTimeStamp(ts);
-        imu_ptr->setData(data_imu);
-
-        // process data in capture
-        imu_ptr->getTimeStamp();
-        sen_imu->process(imu_ptr);
-
-        if(ts.get() == t_odom.get())
-        {
-            // PROCESS ODOM 3D DATA
-            mot_ptr->setTimeStamp(t_odom);
-            mot_ptr->setData(data_odom3D);
-            sen_odom3D->process(mot_ptr);
-
-            //prepare next odometry measurement if there is any
-            odom_data_input >> input_clock >> data_odom3D[0] >> data_odom3D[1] >> data_odom3D[2] >> data_odom3D[3] >> data_odom3D[4] >> data_odom3D[5];
-            t_odom.set(input_clock);
-        }
-
-        #ifdef KF0_EVOLUTION
-        
-        if( (ts.get() - t.get()) >= 0.05 )
-        {
-            t = ts;
-            //std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
-        
-            Eigen::VectorXs frm_state(16);
-            frm_state = origin_KF->getState();
-
-            KF0_evolution << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
-                << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
-                << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
-                << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15) << std::endl;
-        }
-
-        #endif
-    }
-
-    clock_t end = clock();
-    FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-
-    //closing file
-    imu_data_input.close();
-    odom_data_input.close();
-
-    #ifdef KF0_EVOLUTION
-    KF0_evolution.close();
-    #endif
-
-    //===================================================== END{PROCESS DATA}
-
-    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
-    // Final state
-    std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
-    std::cout << "Initial    state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << x_origin.head(16).transpose() << std::endl;
-    std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
-    std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
-    std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
-    << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
-
-    // Print statistics
-    std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
-    std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-    TimeStamp t0, tf;
-    t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
-    tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-    std::cout << "t0        : " << t0.get() << " s" << std::endl;
-    std::cout << "tf        : " << tf.get() << " s" << std::endl;
-    std::cout << "duration  : " << tf-t0 << " s" << std::endl;
-    std::cout << "N samples : " << N << std::endl;
-    std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
-    std::cout << "CPU time  : " << elapsed_secs << " s" << std::endl;
-    std::cout << "s/integr  : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
-    std::cout << "integr/s  : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
-    //fix parts of the problem if needed
-    origin_KF->getP()->fix();
-    origin_KF->getO()->fix();
-    origin_KF->getV()->fix();
-
-    
-    std::cout << "\t\t\t ______solving______" << std::endl;
-    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport
-    std::cout << report << std::endl;
-
-    last_KF->getAccBias()->fix();
-    last_KF->getGyroBias()->fix();
-
-    std::cout << "\t\t\t solving after fixBias" << std::endl;
-    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport
-    std::cout << report << std::endl;
-    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
-    std::cout << "\t\t\t ______solved______" << std::endl;
-
-    wolf_problem_ptr_->print(4,1,1,1);
-
-    #ifdef DEBUG_RESULTS
-    Eigen::VectorXs frm_state(16);
-    Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev;
-    Eigen::MatrixXs covX(16,16);
-    Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
-
-    wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
-    for(FrameBasePtr frm_ptr : frame_list)
-    {
-        if(frm_ptr->isKey())
-        {   
-            //prepare needed variables
-            FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
-            frm_state = frmIMU_ptr->getState();
-            ts = frmIMU_ptr->getTimeStamp();
-
-            //get data from covariance blocks
-            wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
-            wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
-            covX.block(7,7,3,3) = cov3;
-            wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
-            covX.block(10,10,3,3) = cov3;
-            wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
-            covX.block(13,13,3,3) = cov3;
-            for(int i = 0; i<16; i++)
-                cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
-            debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
-            << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
-            << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
-            << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15)
-            << "\t" << cov_stdev(0) << "\t" << cov_stdev(1) << "\t" << cov_stdev(2)
-            << "\t" << cov_stdev(3) << "\t" << cov_stdev(4) << "\t" << cov_stdev(5) << "\t" << cov_stdev(6)
-            << "\t" << cov_stdev(7) << "\t" << cov_stdev(8) << "\t" << cov_stdev(9)
-            << "\t" << cov_stdev(10) << "\t" << cov_stdev(11) << "\t" << cov_stdev(12) << "\t" << cov_stdev(13) << "\t" << cov_stdev(14) << "\t" << cov_stdev(15) << std::endl;
-        }
-    }
-
-    //trials to print all factorIMUs' residuals
-    Eigen::Matrix<wolf::Scalar,15,1> IMU_residuals;
-    Eigen::Vector3s p1(Eigen::Vector3s::Zero());
-    Eigen::Vector4s q1_vec(Eigen::Vector4s::Zero());
-    Eigen::Map<Quaternions> q1(q1_vec.data());
-    Eigen::Vector3s v1(Eigen::Vector3s::Zero());
-    Eigen::Vector3s ab1(Eigen::Vector3s::Zero());
-    Eigen::Vector3s wb1(Eigen::Vector3s::Zero());
-    Eigen::Vector3s p2(Eigen::Vector3s::Zero());
-    Eigen::Vector4s q2_vec(Eigen::Vector4s::Zero());
-    Eigen::Map<Quaternions> q2(q2_vec.data());
-    Eigen::Vector3s v2(Eigen::Vector3s::Zero());
-    Eigen::Vector3s ab2(Eigen::Vector3s::Zero());
-    Eigen::Vector3s wb2(Eigen::Vector3s::Zero());
-
-    for(FrameBasePtr frm_ptr : frame_list)
-    {
-        if(frm_ptr->isKey())
-        {
-            FactorBasePtrList fac_list =  frm_ptr->getConstrainedByList();
-            for(FactorBasePtr fac_ptr : fac_list)
-            {
-                if(fac_ptr->getTypeId() == FAC_IMU)
-                {
-                    //Eigen::VectorXs prev_KF_state(fac_ptr->getFrameOther()->getState());
-                    //Eigen::VectorXs curr_KF_state(fac_ptr->getFeature()->getFrame()->getState());
-                    p1      = fac_ptr->getFrameOther()->getP()->getState();
-                    q1_vec  = fac_ptr->getFrameOther()->getO()->getState();
-                    v1      = fac_ptr->getFrameOther()->getV()->getState();
-                    ab1     = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getAccBias()->getState();
-                    wb1     = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getGyroBias()->getState();
-
-                    p2      = fac_ptr->getFeature()->getFrame()->getP()->getState();
-                    q2_vec  = fac_ptr->getFeature()->getFrame()->getO()->getState();
-                    v2      = fac_ptr->getFeature()->getFrame()->getV()->getState();
-                    ab2     = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getAccBias()->getState();
-                    wb2     = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getGyroBias()->getState();
-
-                    std::static_pointer_cast<FactorIMU>(fac_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
-                    std::cout << "IMU residuals : " << IMU_residuals.transpose() << std::endl;
-                }
-            }
-        }
-    }
-
-    debug_results.close();
-    WOLF_WARN("WARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)")
-
-    #endif
-
-    return 0;
-
-}
-
-int _kbhit()
-{
-    struct timeval tv;
-    fd_set fds;
-    tv.tv_sec = 0;
-    tv.tv_usec = 0;
-    FD_ZERO(&fds);
-    FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
-    select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
-    return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_kf_callback.cpp b/demos/demo_kf_callback.cpp
deleted file mode 100644
index 9e01558e3bc013c65d553d7e213785fdacfaaf96..0000000000000000000000000000000000000000
--- a/demos/demo_kf_callback.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * test_kf_callback.cpp
- *
- *  Created on: Nov 6, 2016
- *      Author: jsola
- */
-
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
-#include "core/capture/capture_void.h"
-
-int main()
-{
-    using namespace wolf;
-    using namespace Eigen;
-    using namespace std;
-
-    ProblemPtr problem = Problem::create("PO", 2);
-
-    SensorBasePtr sen_odo    = problem->installSensor   ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
-    ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
-    params_odo->max_time_span = 2;
-    params_odo->angle_turned = M_PI; // 180 degrees turn
-    ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", sen_odo, params_odo);
-    prc_odo->setTimeTolerance(0.1);
-
-    SensorBasePtr sen_ftr    = problem->installSensor   ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
-    ProcessorParamsTrackerFeaturePtr params_trk = std::make_shared<ProcessorParamsTrackerFeature>();
-    params_trk->max_new_features = 4;
-    params_trk->min_features_for_keyframe = 7;
-    params_trk->time_tolerance = 0.5;
-    shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(params_trk);
-    prc_ftr->setName("tracker");
-    sen_ftr->addProcessor(prc_ftr);
-    prc_ftr->setTimeTolerance(0.1);
-
-    cout << "Motion sensor    : " << problem->getProcessorMotion()->getSensor()->getName() << endl;
-    cout << "Motion processor : " << problem->getProcessorMotion()->getName() << endl;
-
-    TimeStamp t(0);
-    cout << "=======================\n>> TIME: " << t.get() << endl;
-    Vector3s x({0,0,0});
-    Matrix3s P; P.setZero();
-    problem->setPrior(x, P, t, 0.01);
-
-    cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
-
-    Vector2s odo_data;  odo_data << .1, (M_PI / 10);
-
-    problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
-
-    Scalar dt = 1;
-    for (auto i = 0; i < 4; i++)
-    {
-
-        cout << "Tracker----------------" << endl;
-        sen_ftr->process(make_shared<CaptureVoid>(t, sen_ftr));
-        problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
-
-        t += dt;
-        cout << "=======================\n>> TIME: " << t.get() << endl;
-
-        cout << "Motion-----------------" << endl;
-        sen_odo->process(make_shared<CaptureMotion>("ODOM 2D", t, sen_odo, odo_data, 3, 3, nullptr));
-        cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
-        problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
-
-    }
-
-    return 0;
-}
diff --git a/demos/demo_list_remove.cpp b/demos/demo_list_remove.cpp
deleted file mode 100644
index b0795d0a687e92b774c50de49c2645a488676ced..0000000000000000000000000000000000000000
--- a/demos/demo_list_remove.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * test_list_remove.cpp
- *
- *  Created on: Nov 19, 2016
- *      Author: jsola
- */
-
-#include <memory>
-#include <list>
-#include <algorithm>
-#include <iostream>
-
-int main()
-{
-    using std::list;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::cout;
-    using std::endl;
-
-    typedef shared_ptr<int> IntPtr;
-
-    list<IntPtr> L;
-
-    L.push_back(make_shared<int>(0));
-    L.push_back(make_shared<int>(1));
-    L.push_back(make_shared<int>(2));
-
-    cout << "size: " << L.size() << endl;
-
-//    for (auto p : L)
-//    {
-//        cout << "removing " << *p << endl;
-//        L.remove(p);
-//        cout << "size: " << L.size() << endl;
-//    }
-
-//    list<IntPtr>::iterator it = L.begin();
-//    while (it != L.end())
-//    {
-//        cout << "removing " << **it << endl;
-//        L.erase(it);
-//        it = L.begin();
-//        cout << "size: " << L.size() << endl;
-//    }
-
-    list<IntPtr> L_black;
-    for (auto p : L)
-    {
-        cout << "marking " << *p << endl;
-        L_black.push_back(p);
-        cout << "size: " << L.size() << endl;
-    }
-    for (auto p : L_black)
-    {
-        cout << "removing " << *p << endl;
-        L.remove(p);
-        cout << "size: " << L.size() << endl;
-    }
-    return 0;
-}
diff --git a/demos/demo_matrix_prod.cpp b/demos/demo_matrix_prod.cpp
deleted file mode 100644
index b03068283ac33b65a430534a3deb8cf0856195ed..0000000000000000000000000000000000000000
--- a/demos/demo_matrix_prod.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-/**
- * \file test_matrix_prod.cpp
- *
- *  Created on: May 26, 2016
- *      \author: jsola
- */
-
-#include "eigen3/Eigen/Dense"
-
-//std includes
-#include <ctime>
-#include <iostream>
-#include <iomanip>
-
-#include <eigen3/Eigen/StdVector>
-using namespace Eigen;
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,1,1,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,2,2,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,3,3,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,4,4,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,5,5,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,7,7,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,8,8,RowMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,9,RowMajor>)
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,1,1,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,2,2,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,3,3,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,4,4,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,5,5,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,7,7,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,8,8,ColMajor>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,9,ColMajor>)
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,2,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,3,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,4,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,5,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,7,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,8,1>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,1>)
-
-#define DECLARE_MATRICES(s) \
-        Matrix<double, s, s, RowMajor> R1, R2, Ro; \
-        Matrix<double, s, s, ColMajor> C1, C2, Co;
-
-#define INIT_MATRICES(s) \
-        R1.setRandom(s, s);\
-        R2.setRandom(s, s);\
-        C1.setRandom(s, s);\
-        C2.setRandom(s, s);\
-        Ro.setRandom(s, s);\
-        Co.setRandom(s, s);
-
-#define LOOP_MATRIX(N,Mo,M1,M2) \
-        for (int i = 0; i < N; i++) \
-        { \
-            Mo = M1 * M2; \
-            M1(2,2) = Mo(2,2); \
-        }
-
-#define EVALUATE_MATRIX(N,Mo,M1,M2) \
-        t0 = clock(); \
-        LOOP_MATRIX(N,Mo,M1,M2) \
-        t1 = clock(); \
-        std::cout << std::setw(15) << Mo(2,2) << "\t";
-
-#define EVALUATE_ALL \
-        EVALUATE_MATRIX(N, Ro, R1, R2)\
-        std::cout << "Time Ro = R * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Ro, R1, C2)\
-        std::cout << "Time Ro = R * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Ro, C1, R2)\
-        std::cout << "Time Ro = C * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Ro, C1, C2)\
-        std::cout << "Time Ro = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Co, R1, R2)\
-        std::cout << "Time Co = R * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Co, R1, C2)\
-        std::cout << "Time Co = R * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Co, C1, R2)\
-        std::cout << "Time Co = C * R: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;\
-        EVALUATE_MATRIX(N, Co, C1, C2)\
-        std::cout << "Time Co = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N \
-        << "ns <-- this is the Eigen default!" << std::endl;
-
-/**
- * We multiply matrices and see how long it takes.
- * We compare different combinations of row-major and column-major to see which one is the fastest.
- * We can select the matrix size.
- */
-int main()
-{
-    using namespace Eigen;
-
-    int N = 100*1000;
-    const int S = 6;
-    Matrix<double, 16, S - 3 + 1> results;
-    clock_t t0, t1;
-
-    // All dynamic sizes
-    {
-        Matrix<double, Dynamic, Dynamic, RowMajor> R1, R2, Ro;
-        Matrix<double, Dynamic, Dynamic, ColMajor> C1, C2, Co;
-
-        for (int s = 3; s <= S; s++)
-        {
-            std::cout << "Timings for dynamic matrix product. R: row major matrix. C: column major matrix. " << s << "x"
-                    << s << " matrices." << std::endl;
-
-            INIT_MATRICES(s)
-            EVALUATE_ALL
-
-        }
-    }
-    // Statics, one by one
-    {
-        const int s = 3;
-        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
-                << " matrices." << std::endl;
-
-        DECLARE_MATRICES(s)
-        INIT_MATRICES(s)
-        EVALUATE_ALL
-    }
-    {
-        const int s = 4;
-        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
-                << " matrices." << std::endl;
-
-        DECLARE_MATRICES(s)
-        INIT_MATRICES(s)
-        EVALUATE_ALL
-    }
-    {
-        const int s = 5;
-        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
-                << " matrices." << std::endl;
-
-        DECLARE_MATRICES(s)
-        INIT_MATRICES(s)
-        EVALUATE_ALL
-    }
-    {
-        const int s = 6;
-        std::cout << "Timings for static matrix product. R: row major matrix. C: column major matrix. " << s << "x" << s
-                << " matrices." << std::endl;
-
-        DECLARE_MATRICES(s)
-        INIT_MATRICES(s)
-        EVALUATE_ALL
-    }
-
-    std::cout << "Test q and R rotations" << std::endl;
-    Eigen::Quaterniond q(Eigen::Vector4d::Random().normalized());
-    Eigen::Matrix3d R = q.matrix();
-    Eigen::Vector3d v0; v0.setRandom(); v0.normalize(); double v0n = v0.norm();
-    Eigen::Vector3d v;
-
-    N *= 100;
-
-    v = v0;
-    t0 = clock();
-    for (int i = 0; i < N; i++)
-    {
-        v = R * v;
-    }
-    t1 = clock();
-    std::cout << "Time w = R * v: " << (double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;
-    std::cout << "v norm change: " << 10*logl((long double)v.norm()/(long double)v0n) << " dB" << std::endl;
-
-    v = v0;
-    t0 = clock();
-    for (int i = 0; i < N; i++)
-    {
-        v = q * v;
-    }
-    t1 = clock();
-    std::cout << "Time w = q * v: " << (double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N << "ns" << std::endl;
-    std::cout << "v norm change: " << 10*logl((long double)v.norm()/(long double)v0n) << " dB" << std::endl;
-    return 0;
-}
-
diff --git a/demos/demo_mpu.cpp b/demos/demo_mpu.cpp
deleted file mode 100644
index f2d5bbade62637c592b5f2dd0cf2341d825dded8..0000000000000000000000000000000000000000
--- a/demos/demo_mpu.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/**
- * \file test_mpu.cpp
- *
- *  Created on: Oct 4, 2016
- *      \author: AtDinesh
- */
-
- //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-#include <termios.h>
-#include <fcntl.h>
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-
-#define DEBUG_RESULTS
-#define FROM_FILE
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    #ifdef FROM_FILE
-        std::ifstream data_file;
-        const char * filename;
-
-        if (argc < 2)
-        {
-            std::cout << "Missing input argument! : needs 1 argument (path to data file)." << std::endl;
-            return 1;
-        }
-        else
-        {
-            filename = argv[1];
-            data_file.open(filename);
-            std::cout << "file: " << filename << std::endl;
-
-            std::string dummy;
-            getline(data_file, dummy);
-
-        if(!data_file.is_open()){
-            std::cerr << "Failed to open data files... Exiting" << std::endl;
-            return 1;
-        }
-    }
-    #else
-    int fd,n;
-    ///prepare MPU here
-    if (argc < 2)
-    {
-        std::cout << "Missing input argument! : needs 1 argument : way to MPU device. (usually /dev/ttyACM#)\n Please make sure that you have rights to access the device and that your user belongs to the dialout group." << std::endl;
-        return 1;
-    }
-    unsigned char buf[64] = {0};
-	wolf::Scalar gravity = 9.81;
-	wolf::Scalar sec_to_rad = 3.14159265359/180.0;
-	wolf::Scalar accel_LSB = 1.0/8192.0; // = 4.0/32768.0
-	wolf::Scalar gyro_LSB = 1.0/131.0; // = 250.0/32768.0
-    wolf::Scalar accel_LSB_g = accel_LSB * gravity;
-	wolf::Scalar gyro_LSB_rad = gyro_LSB * sec_to_rad;
-    //wolf::Scalar Ax, Ay, Az, Gx, Gy, Gz;
-
-    struct termios toptions;
-    //open serial port
-    std::cout << "open port...\n" << std::endl;
-    fd = open(argv[1], O_RDWR | O_NOCTTY);
-    if (fd != -1)
-        std::cout << "MPU openned successfully! \n" << std::endl;
-    else
-        std::cout << "MPU could not be openned... \n" << std::endl;
-
-    //configuring termios
-    tcgetattr(fd, &toptions);
-    cfsetispeed(&toptions, B1000000);
-    cfsetospeed(&toptions, B1000000);
-    toptions.c_cflag     |= (CLOCAL | CREAD);
-    toptions.c_lflag     &= ~(ICANON | ECHO | ECHOE | ISIG);
-    toptions.c_oflag     &= ~OPOST;
-    toptions.c_cc[VMIN]  = 0;
-    toptions.c_cc[VTIME] = 10;
-    tcsetattr(fd, TCSANOW, &toptions);
-    #endif
-
-    #ifdef DEBUG_RESULTS
-    std::ofstream debug_results;
-    debug_results.open("debug_results.dat");
-    if(debug_results)
-        debug_results << "%%TimeStamp\t"
-                      << "dp_x\t" << "dp_y\t" << "dp_z\t" << "dv_x\t" << "dv_y\t" << "dv_z\t" << "dq_x\t" << "dq_y\t" << "dq_z\t" << "dq_w\t"
-                      << "Dp_x\t" << "Dp_y\t" << "Dp_z\t" << "Dv_x\t" << "Dv_y\t" << "Dv_z\t" << "Dq_x\t" << "Dq_y\t" << "Dq_z\t" << "Dq_w\t"
-                      << "X_x\t" << "X_y\t" << "X_z\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << std::endl;
-    #endif
-
-    // Wolf problem
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
-    Eigen::VectorXs IMU_extrinsics(7);
-    IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-    SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, IntrinsicsBase());
-    wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
-    // Time and data variables
-    TimeStamp t;
-    Eigen::Vector6s data_;
-    Scalar mpu_clock = 0;
-
-    t.set(mpu_clock * 0.0001); // clock in 0,1 ms ticks
-
-    // Set the origin
-    Eigen::VectorXs x0(16);
-    x0 << 0,0,0,  0,0,0,  1,0,0,0,  0,0,.001,  0,0,.002; // Try some non-zero biases
-    wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
-
-    // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
-    CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) );
-
-    // main loop
-    using namespace std;
-    clock_t begin = clock();
-    std::cout << "\n\t\t\t\tENTERING MAIN LOOP - Please press ENTER to exit loop\n" << std::endl;
-
-    #ifdef FROM_FILE
-    while(!data_file.eof()){
-        // read new data
-        data_file >> mpu_clock >> data_[0] >> data_[1] >> data_[2] >> data_[3] >> data_[4] >> data_[5];
-        t.set(mpu_clock); //
-    #else
-    while(!_kbhit()){
-        // read new data
-        do n = read(fd, buf, 1);//READ IT
-        while (buf[0]!=0x47); //control character has been found
-        n = read(fd, buf, 12);//read the data
-        if (n>3){ //construct data_ from IMU input
-			data_(0)   = (wolf::Scalar)((int16_t)((buf[1]<<8)|buf[0]))*accel_LSB_g;
-			data_(1)   = (wolf::Scalar)((int16_t)((buf[3]<<8)|buf[2]))*accel_LSB_g;
-			data_(2)   = (wolf::Scalar)((int16_t)((buf[5]<<8)|buf[4]))*accel_LSB_g;
-			data_(3)   = (wolf::Scalar)((int16_t)((buf[7]<<8)|buf[6]))*gyro_LSB_rad;
-			data_(4)   = (wolf::Scalar)((int16_t)((buf[9]<<8)|buf[8]))*gyro_LSB_rad;
-			data_(5)   = (wolf::Scalar)((int16_t)((buf[11]<<8)|buf[10]))*gyro_LSB_rad;
-            mpu_clock += 0.001;
-            t.set(mpu_clock);
-        }
-        #endif
-
-        // assign data to capture
-        imu_ptr->setData(data_);
-        imu_ptr->setTimeStamp(t);
-
-        // process data in capture
-        sensor_ptr->process(imu_ptr);
-
-        #ifdef DEBUG_RESULTS
-
-        Eigen::VectorXs delta_debug;
-        Eigen::VectorXs delta_integr_debug;
-        Eigen::VectorXs x_debug;
-        TimeStamp ts;
-
-        delta_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_;
-        delta_integr_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
-        x_debug = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-        ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-
-        if(debug_results)
-            debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
-            << delta_debug(5) << "\t" << delta_debug(6) << "\t" << delta_debug(7) << "\t" << delta_debug(8) << "\t" << delta_debug(9) << "\t"
-            << delta_integr_debug(0) << "\t" << delta_integr_debug(1) << "\t" << delta_integr_debug(2) << "\t" << delta_integr_debug(3) << "\t" << delta_integr_debug(4) << "\t"
-            << delta_integr_debug(5) << "\t" << delta_integr_debug(6) << "\t" << delta_integr_debug(7) << "\t" << delta_integr_debug(8) << "\t" << delta_integr_debug(9) << "\t"
-            << x_debug(0) << "\t" << x_debug(1) << "\t" << x_debug(2) << "\t" << x_debug(3) << "\t" << x_debug(4) << "\t"
-            << x_debug(5) << "\t" << x_debug(6) << "\t" << x_debug(7) << "\t" << x_debug(8) << "\t" << x_debug(9) << "\n";
-        #endif
-    }
-
-    clock_t end = clock();
-    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
-    // Final state
-    std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
-    std::cout << "Initial    state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << x0.head(16).transpose() << std::endl;
-    std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
-    std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
-    std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
-    << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
-    // Print statistics
-    std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
-    std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-#ifdef DEBUG_RESULTS
-    std::cout << "\t\tWARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)" << std::endl;
-    debug_results.close();
-#endif
-
-    TimeStamp t0, tf;
-    t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
-    tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-    std::cout << "t0        : " << t0.get() << " s" << std::endl;
-    std::cout << "tf        : " << tf.get() << " s" << std::endl;
-    std::cout << "duration  : " << tf-t0 << " s" << std::endl;
-    std::cout << "N samples : " << N << std::endl;
-    std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
-    std::cout << "CPU time  : " << elapsed_secs << " s" << std::endl;
-    std::cout << "s/integr  : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
-    std::cout << "integr/s  : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
-    return 0;
-
-}
-
-int _kbhit()
-{
-    struct timeval tv;
-    fd_set fds;
-    tv.tv_sec = 0;
-    tv.tv_usec = 0;
-    FD_ZERO(&fds);
-    FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
-    select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
-    return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
deleted file mode 100644
index ec77aedeee3242daffe10d89c1444fa9ea1bf1f9..0000000000000000000000000000000000000000
--- a/demos/demo_processor_imu.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-/**
- * \file test_processor_imu.cpp
- *
- *  Created on: Apr 12, 2016
- *      \author: dtsbourg
- */
-
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    std::cout << std::endl << "==================== processor IMU test ======================" << std::endl;
-
-    //load files containing accelerometer and gyroscope data
-    std::ifstream data_file_acc;
-    std::ifstream data_file_gyro;
-    const char * filename_acc;
-    const char * filename_gyro;
-
-    //prepare creation of file if DEBUG_RESULTS activated
-#ifdef DEBUG_RESULTS
-    std::ofstream debug_results;
-    debug_results.open("debug_results.dat");
-    if(debug_results)
-        debug_results << "%%TimeStamp\t"
-                      << "dp_x\t" << "dp_y\t" << "dp_z\t" << "dq_x\t" << "dq_y\t" << "dq_z\t" << "dq_w\t" << "dv_x\t" << "dv_y\t" << "dv_z\t"
-                      << "Dp_x\t" << "Dp_y\t" << "Dp_z\t" << "Dq_x\t" << "Dq_y\t" << "Dq_z\t" << "Dq_w\t" << "Dv_x\t" << "Dv_y\t" << "Dv_z\t"
-                      << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t" << std::endl;
-#endif
-
-    if (argc < 3)
-    {
-        std::cout << "Missing input argument! : needs 2 arguments (path to accelerometer file and path to gyroscope data)." << std::endl;
-    }
-    else
-    {
-        filename_acc = argv[1];
-        filename_gyro = argv[2];
-        data_file_acc.open(filename_acc);
-        data_file_gyro.open(filename_gyro);
-        std::cout << "Acc  file: " << filename_acc << std::endl;
-        std::cout << "Gyro file: " << filename_gyro << std::endl;
-
-        std::string dummy;
-        getline(data_file_acc, dummy); getline(data_file_gyro, dummy);
-
-        if(!data_file_acc.is_open() || !data_file_gyro.is_open()){
-            std::cerr << "Failed to open data files... Exiting" << std::endl;
-            return 1;
-        }
-    }
-
-    // Wolf problem
-    ProblemPtr problem_ptr_ = Problem::create("PQVBB 3D");
-    Eigen::VectorXs extrinsics(7);
-    extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-    SensorBasePtr sensor_ptr = problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBasePtr());
-    problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
-    // Time and data variables
-    TimeStamp t;
-    Scalar mti_clock, tmp;
-    Eigen::Vector6s data;
-    Eigen::Matrix6s data_cov;
-    data_cov.setIdentity();
-    data_cov.topLeftCorner(3,3)     *= 0.01;
-    data_cov.bottomRightCorner(3,3) *= 0.01;
-
-    // Get initial data
-    data_file_acc >> mti_clock >> data[0] >> data[1] >> data[2];
-    data_file_gyro >> tmp >> data[3] >> data[4] >> data[5];
-    t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
-
-    // Set the origin
-    Eigen::VectorXs x0(16);
-    x0 << 0,0,0,  0,0,0,1,  0,0,0,  0,0,0,  0,0,0; // Try some non-zero biases
-    problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
-
-    // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
-    CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
-
-//    problem_ptr_->print();
-
-    std::cout << "Main loop -----------" << std::endl;
-
-    // main loop
-    using namespace std;
-    clock_t begin = clock();
-    int n = 1;
-    while(!data_file_acc.eof() && n < 5000){
-        n++;
-
-        // read new data
-        data_file_acc >> mti_clock  >> data[0] >> data[1] >> data[2];
-        data_file_gyro >> tmp       >> data[3] >> data[4] >> data[5];
-        t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
-
-//        data.setZero();
-//        data(2) = 9.8;
-
-        // assign data to capture
-        imu_ptr->setData(data);
-        imu_ptr->setTimeStamp(t);
-
-        // process data in capture
-        sensor_ptr->process(imu_ptr);
-
-#ifdef DEBUG_RESULTS
-
-        // --- print to screen ----
-
-        std::cout << "Current    data : " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
-                << data.transpose() << std::endl;
-
-        std::cout << "Current    delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
-                << problem_ptr_->getProcessorMotion()->getMotion().delta_.transpose() << std::endl;
-
-        std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
-                << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
-
-        Eigen::VectorXs x = problem_ptr_->getProcessorMotion()->getCurrentState();
-
-        std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
-                << x.head(10).transpose() << std::endl;
-
-        std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
-                << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
-        std::cout << std::endl;
-
-//#ifdef DEBUG_RESULTS
-        // ----- dump to file -----
-
-        Eigen::VectorXs delta_debug;
-        Eigen::VectorXs delta_integr_debug;
-        Eigen::VectorXs x_debug;
-        TimeStamp ts;
-
-        delta_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_;
-        delta_integr_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
-        x_debug = problem_ptr_->getProcessorMotion()->getCurrentState();
-        ts = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-
-        if(debug_results)
-            debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
-            << delta_debug(5) << "\t" << delta_debug(6) << "\t" << delta_debug(7) << "\t" << delta_debug(8) << "\t" << delta_debug(9) << "\t"
-            << delta_integr_debug(0) << "\t" << delta_integr_debug(1) << "\t" << delta_integr_debug(2) << "\t" << delta_integr_debug(3) << "\t" << delta_integr_debug(4) << "\t"
-            << delta_integr_debug(5) << "\t" << delta_integr_debug(6) << "\t" << delta_integr_debug(7) << "\t" << delta_integr_debug(8) << "\t" << delta_integr_debug(9) << "\t"
-            << x_debug(0) << "\t" << x_debug(1) << "\t" << x_debug(2) << "\t" << x_debug(3) << "\t" << x_debug(4) << "\t"
-            << x_debug(5) << "\t" << x_debug(6) << "\t" << x_debug(7) << "\t" << x_debug(8) << "\t" << x_debug(9) << "\n";
-#endif
-
-    }
-    clock_t end = clock();
-    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
-    // Final state
-    std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
-    std::cout << "Initial    state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << x0.head(16).transpose() << std::endl;
-    std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
-    std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
-    << problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
-//    std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
-//    << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
-    // Print statistics
-    std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
-    std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-#ifdef DEBUG_RESULTS
-    std::cout << "\t\tWARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)" << std::endl;
-    debug_results.close();
-#endif
-
-    TimeStamp t0, tf;
-    t0 = problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
-    tf = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
-    int N = problem_ptr_->getProcessorMotion()->getBuffer().get().size();
-    std::cout << "t0        : " << t0.get() << " s" << std::endl;
-    std::cout << "tf        : " << tf.get() << " s" << std::endl;
-    std::cout << "duration  : " << tf-t0 << " s" << std::endl;
-    std::cout << "N samples : " << N << std::endl;
-    std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
-    std::cout << "CPU time  : " << elapsed_secs << " s" << std::endl;
-    std::cout << "s/integr  : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
-    std::cout << "integr/s  : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
-    // close data files
-    data_file_acc.close(); // no impact on leaks
-    data_file_gyro.close();
-
-    return 0;
-}
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
deleted file mode 100644
index 988244d1598d3a8a9a1bdebf0144266b6744044b..0000000000000000000000000000000000000000
--- a/demos/demo_processor_imu_jacobians.cpp
+++ /dev/null
@@ -1,418 +0,0 @@
-/**
- * \file test_processor_imu_jacobians.cpp
- *
- *  Created on: Sep 26, 2016
- *      \author: AtDinesh
- */
-
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include <test/processor_IMU_UnitTester.h>
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-using namespace wolf;
-
-void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0);
-void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place );
-
-int main(int argc, char** argv)
-{
-    using namespace wolf;
-
-    std::cout << std::endl << "==================== processor IMU : Checking Jacobians ======================" << std::endl;
-
-    TimeStamp t;
-    Eigen::Vector6s data_;
-    wolf::Scalar deg_to_rad = 3.14159265359/180.0;
-    data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
-
-    // Wolf problem
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
-    Eigen::VectorXs IMU_extrinsics(7);
-    IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-    //SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, nullptr);
-    //wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
-
-    // Set the origin
-    t.set(0.0001); // clock in 0,1 ms ticks
-    Eigen::VectorXs x0(16);
-    x0 << 0,1,0,   0,0,0,1,  1,0,0,  0,0,.000,  0,0,.000; // P Q V B B
-
-    //wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
-
-    //CaptureIMU* imu_ptr;
-
-    ProcessorIMU_UnitTester processor_imu;
-    //processor_imu.setOrigin(x0, t);
-    wolf::Scalar ddelta_bias = 0.00000001;
-    wolf::Scalar dt = 0.001;
-
-    //defining a random Delta to begin with (not to use Origin point)
-    Eigen::Matrix<wolf::Scalar,10,1> Delta0;
-    Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
-    Delta0.head<3>() = Delta0.head<3>()*100;
-    Delta0.tail<3>() = Delta0.tail<3>()*10;
-    Eigen::Vector3s ang0, ang;
-    ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad; 
-    //Delta0 << 0,0,0, 0,0,0,1, 0,0,0;
-    Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
-    Delta0_quat = v2q(ang0);
-    Delta0_quat.normalize();
-    ang = q2v(Delta0_quat);
-
-    std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
-    std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
-
-    struct IMU_jac_bias bias_jac = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
-
-    Eigen::Map<Eigen::Quaternions> Dq0(NULL);
-    Eigen::Map<Eigen::Quaternions> dq0(NULL);
-    Eigen::Map<Eigen::Quaternions> Dq_noisy(NULL);
-    Eigen::Map<Eigen::Quaternions> dq_noisy(NULL);
-    Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
-
-    /* IMU_jac_deltas struct form :
-    contains vectors of size 7 :
-    Elements at place 0 are those not affected by the bias noise that we add (da_bx,..., dw_bx,... ).
-                place 1 : added da_bx in data         place 2 : added da_by in data       place 3 : added da_bz in data
-                place 4 : added dw_bx in data         place 5 : added dw_by in data       place 6 : added dw_bz in data
-    */
-
-    Eigen::Matrix3s dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dab, dDq_dwb;
-
-    /*
-        dDp_dab = [dDp_dab_x, dDp_dab_y, dDp_dab_z]
-        dDp_dab_x = (dDp(ab_x + dab_x, ab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_x
-        dDp_dab_x = (dDp(ab_x, ab_y + dab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_y
-        dDp_dab_x = (dDp(ab_x, ab_y, ab_z + dab_z) - dDp(ab_x,ab_y,ab_z)) / dab_z
-
-        similar for dDv_dab
-        note dDp_dab_x, dDp_dab_y, dDp_dab_z, dDv_dab_x, dDv_dab_y, dDv_dab_z are 3x1 vectors !
-
-        dDq_dab = 0_{3x3}
-        dDq_dwb = [dDq_dwb_x, dDq_dwb_y, dDq_dwb_z]
-        dDq_dwb_x = log( dR(wb).transpose() * dR(wb - dwb_x))/dwb_x
-                  = log( dR(wb).transpose() * exp((wx - wbx - dwb_x)dt, (wy - wby)dt, (wy - wby)dt))/dwb_x
-        dDq_dwb_y = log( dR(wb).transpose() * dR(wb - dwb_y))/dwb_y
-        dDq_dwb_z = log( dR(wb).transpose() * dR(wb + dwb_z))/dwb_z
-
-        Note : dDq_dwb must be computed recursively ! So comparing the one returned by processor_imu and the numerical 
-        one will have no sense if we aredoing this from a random Delta. The Delta here should be the origin.
-        dDq_dwb_ = dR.tr()*dDq_dwb - Jr(wdt)*dt
-        Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt 
-     */
-
-     std::cout << "\n input data : \n" << data_ << std::endl;
-
-     new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
-     for(int i=0;i<3;i++){
-         dDp_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
-         dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
-         dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
-         dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
-         new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
-         dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
-
-         new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
-         dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
-         //std::cout << "matrix operation result :" << i << "\n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
-         //std::cout << "matrix operation result to vector :" << i << "\n" << R2v( q_in_1.matrix().transpose() * q_in_2.matrix()) << std::endl;
-     }
-/*     Delta1 = bias_jac.Deltas_noisy_vect_(3);
-     Delta2 = bias_jac.Deltas_noisy_vect_(4);
-     Delta3 = bias_jac.Deltas_noisy_vect_(5);
-
-     if( (Delta1 == Delta2 || Delta1 == Delta3 ))
-        std::cout << "\n problem" << std::endl;
-    else
-        std::cout << "\n no problem" << std::endl;
-    if(Delta2 == Delta3)
-        std::cout << "\n problem" << std::endl;
-    else
-        std::cout << "\n no problem" << std::endl;*/
-
-     //Check the jacobians wrt to bias using finite difference
-
-    if(dDp_dab.isApprox(bias_jac.dDp_dab_, wolf::Constants::EPS) )
-        std::cout<< "dDp_dab_ jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDp_dab_ jacobian is not correct ..." << std::endl;
-        std::cout << "dDp_dab : \n" << dDp_dab << "\n bias_jac.dDp_dab_ :\n" << bias_jac.dDp_dab_ << "\n" << std::endl;
-        std::cout << "dDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab <<  "\n" << std::endl;
-    }
-
-    if(dDv_dab.isApprox(bias_jac.dDv_dab_, wolf::Constants::EPS) )
-        std::cout<< "dDv_dab_ jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDv_dab_ jacobian is not correct ..." << std::endl;
-        std::cout << "dDv_dab_ : \n" << dDv_dab << "\n bias_jac.dDv_dab_ :\n" << bias_jac.dDv_dab_ << "\n" << std::endl;
-        std::cout << "dDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab <<  "\n" << std::endl;
-    }
-
-    if(dDp_dwb.isApprox(bias_jac.dDp_dwb_, wolf::Constants::EPS) )
-        std::cout<< "dDp_dwb_ jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDp_dwb_ jacobian is not correct ..." << std::endl;
-        std::cout << "dDp_dwb_ : \n" << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n" << bias_jac.dDp_dwb_ << "\n" << std::endl;
-        std::cout << "dDp_dwb_a - dDp_dwb_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb <<  "\n" << std::endl;
-    }
-
-    if(dDv_dwb.isApprox(bias_jac.dDv_dwb_, wolf::Constants::EPS) )
-        std::cout<< "dDv_dwb_ jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDv_dwb_ jacobian is not correct ..." << std::endl;
-        std::cout << "dDv_dwb_ : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ << "\n" <<  std::endl;
-        std::cout << "dDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb <<  "\n" << std::endl;
-    }
-
-    if(dDq_dwb.isApprox(bias_jac.dDq_dwb_, wolf::Constants::EPS) )
-        std::cout<< "dDq_dwb_ jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDq_dwb_ jacobian is not correct ..." << std::endl;
-        std::cout << "dDq_dwb_ : \n" << dDq_dwb << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_ << "\n" << std::endl;
-        std::cout << "dDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb <<  "\n" << std::endl;
-    }
-
-    //Check jacobians that are supposed to be Zeros just in case
-    if(dDq_dab.isZero(wolf::Constants::EPS))
-        std::cout<< "dDq_dab_ jacobian is correct (Zero) !" << std::endl;
-    else
-        std::cout<< "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
-
-    /*              Numerical method to check jacobians wrt noise
-
-                                                            dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz]
-    dDp_dPx = ((P + dPx) - P)/dPx = Id
-    dDp_dPy = ((P + dPy) - P)/dPy = Id
-    dDp_dPz = ((P + dPz) - P)/dPz = Id
-
-                                                            dDp_dV = [dDp_dVx, dDp_dVy, dDp_dVz]
-    dDp_dVx = ((V + dVx)*dt - V*dt)/dVx = Id*dt
-    dDp_dVy = ((V + dVy)*dt - V*dt)/dVy = Id*dt
-    dDp_dVz = ((V + dVz)*dt - V*dt)/dVz = Id*dt
-
-                                                            dDp_dO = [dDp_dOx, dDp_dOy, dDp_dOz]
-    dDp_dOx = (( dR(Theta + dThetax)*dp ) - ( dR(Theta)*dp ))/dThetax 
-            = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
-    dDp_dOy = (( dR(Theta) * exp(0,dThetay,0)*dp ) - ( dR(Theta)*dp ))/dThetay
-    dDp_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dp ) - ( dR(Theta)*dp ))/dThetaz
-
-                                                            dDv_dP = [dDv_dPx, dDv_dPy, dDv_dPz] = [0, 0, 0]
-
-                                                            dDv_dV = [dDv_dVx, dDv_dVy, dDv_dVz]
-    dDv_dVx = ((V + dVx) - V)/dVx = Id
-    dDv_dVy = ((V + dVy) - V)/dVy = Id
-    dDv_dVz = ((V + dVz) - V)/dVz = Id
-    
-                                                            dDv_dO = [dDv_dOx, dDv_dOy, dDv_dOz]
-    dDv_dOx = (( dR(Theta + dThetax)*dv ) - ( dR(Theta)*dv ))/dThetax 
-            = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
-    dDv_dOx = (( dR(Theta) * exp(0,dThetay,0)*dv ) - ( dR(Theta)*dv ))/dThetay
-    dDv_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dv ) - ( dR(Theta)*dv ))/dThetaz
-
-                                                            dDp_dp = [dDp_dpx, dDp_dpy, dDp_dpz]
-    dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
-    dDp_dpy = ( dR*(p + dpy) - dR*(p))/dpy
-    dDp_dpz = ( dR*(p + dpz) - dR*(p))/dpy
-
-                                                            dDp_dv = [dDp_dvx, dDp_dvy, dDp_dvz] = [0, 0, 0]
-    
-                                                            dDp_do = [dDp_dox, dDp_doy, dDp_doz] = [0, 0, 0]
-
-                                                            dDv_dp = [dDv_dpx, dDv_dpy, dDv_dpz] = [0, 0, 0]
-                                                            
-                                                            dDv_dv = [dDv_dvx, dDv_dvy, dDv_dvz]
-    dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
-    dDv_dvy = ( dR*(v + dvy) - dR*(v))/dvy
-    dDv_dvz = ( dR*(v + dvz) - dR*(v))/dvz
-
-                                                            dDv_do = [dDv_dox, dDv_doy, dDv_doz] = [0, 0, 0]
-
-                                                            dDo_dp = dDo_dv = dDo_dP = dDo_dV = [0, 0, 0]
-
-                                                            dDo_dO = [dDo_dOx, dDo_dOy, dDo_dOz]
-                                                            
-    dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta+dThetax) * dr(theta) )/dThetax
-            = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
-            = log( (_Delta * _delta).transpose() * (_Delta_noisy * _delta))
-    dDo_dOy = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,dThetay,0)) * dr(theta) )/dThetay
-    dDo_dOz = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,0,dThetaz)) * dr(theta) )/dThetaz
-
-                                                            dDo_do = [dDo_dox, dDo_doy, dDo_doz]
-
-    dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * dr(theta+dthetax) )/dthetax
-            = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
-            = log( (_Delta * _delta).transpose() * (_Delta * _delta_noisy))
-    dDo_doy = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,dthetay,0)) )/dthetay
-    dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
-     */
-
-     //taking care of noise now 
-    Eigen::Matrix<wolf::Scalar,9,1> Delta_noise;
-    Eigen::Matrix<wolf::Scalar,9,1> delta_noise;
-
-    Delta_noise << 0.00000001, 0.00000001, 0.00000001,  0.0001, 0.0001, 0.0001,  0.00000001, 0.00000001, 0.00000001;
-    delta_noise << 0.00000001, 0.00000001, 0.00000001,  0.0001, 0.0001, 0.0001,  0.00000001, 0.00000001, 0.00000001;
-
-    struct IMU_jac_deltas deltas_jac = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
-
-    /* reminder : 
-                            jacobian_delta_preint_vect_                                                            jacobian_delta_vect_
-                            0: + 0,                                                                                 0: + 0
-                            1: +dPx, 2: +dPy, 3: +dPz                                                               1: + dpx, 2: +dpy, 3: +dpz
-                            4: +dOx, 5: +dOy, 6: +dOz                                                               4: + dox, 5: +doy, 6: +doz
-                            7: +dVx, 8: +dVy, 9: +dVz                                                               7: + dvx, 8: +dvy, 9: +dvz
-    */
-
-    Eigen::Matrix3s dDp_dP, dDp_dV, dDp_dO, dDv_dP, dDv_dV, dDv_dO, dDo_dP, dDo_dV, dDo_dO;
-    Eigen::Matrix3s dDp_dp, dDp_dv, dDp_do, dDv_dp, dDv_dv, dDv_do, dDo_dp, dDo_dv, dDo_do; 
-
-    remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
-    
-    for(int i = 0; i < 3; i++){
-
-        //dDp_dPx = ((P + dPx) - P)/dPx
-        dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
-        //Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
-        dDp_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3)*dt - deltas_jac.Delta0_.tail(3)*dt)/Delta_noise(i+6);
-        //dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
-        remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
-        dDp_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix()* deltas_jac.delta0_.head(3)))/Delta_noise(i+3);
-
-        //dDv_dP = [0, 0, 0]
-        //dDv_dVx = ((V + dVx) - V)/dVx
-        dDv_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3) - deltas_jac.Delta0_.tail(3))/Delta_noise(i+6);
-        //dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
-        dDv_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.tail(3)) - (Dq0.matrix()* deltas_jac.delta0_.tail(3)))/Delta_noise(i+3);
-
-        //dDo_dP = dDo_dV = [0, 0, 0]
-        //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
-        dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
-
-        //dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
-        dDp_dp.block<3,1>(0,i) = ( (Dq0.matrix() * deltas_jac.delta_noisy_vect_(i).head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3)) )/delta_noise(i);
-        //dDp_dv = dDp_do = [0, 0, 0]
-
-        //dDv_dp = [0, 0, 0]
-        //dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
-        dDv_dv.block<3,1>(0,i) = ( (Dq0 * deltas_jac.delta_noisy_vect_(i+6).tail(3)) - (Dq0 * deltas_jac.delta0_.tail(3)) )/delta_noise(i+6);
-        //dDv_do = [0, 0, 0]
-
-        //dDo_dp = dDo_dv = [0, 0, 0]
-        //dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
-        dDo_do.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix()) )/Delta_noise(i+3);
-    }
-
-    /* jacobians wrt deltas have PVQ form :
-    dDp_dP = deltas_jac.jacobian_delta_preint_.block(0,0,3,3);    dDp_dV = deltas_jac.jacobian_delta_preint_.block(0,3,3,3);        dDp_dO = deltas_jac.jacobian_delta_preint_.block(0,6,3,3);
-    dDv_dP = deltas_jac.jacobian_delta_preint_.block(3,0,3,3);    dDv_dV = deltas_jac.jacobian_delta_preint_.block(3,3,3,3);        dDv_dO = deltas_jac.jacobian_delta_preint_.block(3,6,3,3);
-    dDo_dP = deltas_jac.jacobian_delta_preint_.block(6,0,3,3);    dDo_dV = deltas_jac.jacobian_delta_preint_.block(6,3,3,3);        dDo_dO = deltas_jac.jacobian_delta_preint_.block(6,6,3,3);
-     */
-
-    if(dDp_dP.isApprox(deltas_jac.jacobian_delta_preint_.block(0,0,3,3), wolf::Constants::EPS) )
-        std::cout<< "dDp_dP jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDp_dP jacobian is not correct ..." << std::endl;
-        std::cout << "dDp_dP : \n" << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) << "\n" << std::endl;
-        std::cout << "dDp_dP_a - dDp_dP : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP <<  "\n" << std::endl;
-    }
-
-    if(dDp_dV.isApprox(deltas_jac.jacobian_delta_preint_.block(0,6,3,3), wolf::Constants::EPS) )
-        std::cout<< "dDp_dV jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDp_dV jacobian is not correct ..." << std::endl;
-        std::cout << "dDp_dV : \n" << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) << "\n" << std::endl;
-        std::cout << "dDp_dV_a - dDp_dV : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV <<  "\n" << std::endl;
-    }
-
-    if(dDp_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(0,3,3,3), wolf::Constants::EPS) )
-        std::cout<< "dDp_dO jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDp_dO jacobian is not correct ..." << std::endl;
-        std::cout << "dDp_dO : \n" << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) << "\n" << std::endl;
-        std::cout << "dDp_dO_a - dDp_dO : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO <<  "\n" << std::endl;
-    }
-
-    if(dDv_dV.isApprox(deltas_jac.jacobian_delta_preint_.block(6,6,3,3), wolf::Constants::EPS) )
-        std::cout<< "dDv_dV jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDv_dV jacobian is not correct ..." << std::endl;
-        std::cout << "dDv_dV : \n" << dDv_dV << "\n deltas_jac.jacobian_delta_preint_.block(6,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) << "\n" << std::endl;
-        std::cout << "dDv_dV_a - dDv_dV : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV <<  "\n" << std::endl;    
-    }
-
-    if(dDv_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(6,3,3,3), wolf::Constants::EPS) )
-        std::cout<< "dDv_dO jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDv_dO jacobian is not correct ..." << std::endl;
-        std::cout << "dDv_dO : \n" << dDv_dO << "\n deltas_jac.jacobian_delta_preint_.block(6,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) << "\n" << std::endl;
-        std::cout << "dDv_dO_a - dDv_dO : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO <<  "\n" << std::endl;
-    }
-
-    if(dDo_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(3,3,3,3), wolf::Constants::EPS) )
-        std::cout<< "dDo_dO jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDo_dO jacobian is not correct ..." << std::endl;
-        std::cout << "dDo_dO : \n" << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) << "\n" << std::endl;
-        std::cout << "dDo_dO_a - dDo_dO : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO <<  "\n" << std::endl;
-    }
-
-     Eigen::Matrix3s dDp_dp_a, dDv_dv_a, dDo_do_a;
-     dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
-     dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
-     dDo_do_a = deltas_jac.jacobian_delta_.block(3,3,3,3);
-
-    if(dDp_dp.isApprox(dDp_dp_a, wolf::Constants::EPS) )
-        std::cout<< "dDp_dp jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDp_dp jacobian is not correct ..." << std::endl;
-        std::cout << "dDp_dp : \n" << dDv_dp << "\n dDp_dp_a :\n" << dDp_dp_a << "\n" << std::endl;
-        std::cout << "dDp_dp_a - dDp_dp : \n" << dDp_dp_a - dDv_dp <<  "\n" << std::endl;
-    }
-
-    if(dDv_dv.isApprox(dDv_dv_a, wolf::Constants::EPS) )
-        std::cout<< "dDv_dv jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDv_dv jacobian is not correct ..." << std::endl;
-        std::cout << "dDv_dv : \n" << dDv_dv << "\n dDv_dv_a :\n" << dDv_dv_a << "\n" << std::endl;
-        std::cout << "dDv_dv_a - dDv_dv : \n" << dDv_dv_a - dDv_dv <<  "\n" << std::endl;
-    }
-
-    if(dDo_do.isApprox(dDo_do_a, wolf::Constants::EPS) )
-        std::cout<< "dDo_do jacobian is correct !" << std::endl;
-    else{
-        std::cout<< "\t\tdDo_do jacobian is not correct ..." << std::endl;
-        std::cout << "dDo_do : \n" << dDo_do << "\n dDo_do_a :\n" << dDo_do_a << "\n" << std::endl;
-        std::cout << "dDo_do_a - dDo_do : \n" << dDo_do_a - dDo_do <<  "\n" << std::endl;
-    }
-
-    return 0;
-}
-
-using namespace wolf;
-
-void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
-
-        new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
-        new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
-}
-
-void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
-    
-    assert(place < _jac_delta.Delta_noisy_vect_.size());
-    new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
-    new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
-}
diff --git a/demos/demo_processor_odom_3D.cpp b/demos/demo_processor_odom_3D.cpp
deleted file mode 100644
index 7cfae793d291d83364261c8996f78d0b64fa3d13..0000000000000000000000000000000000000000
--- a/demos/demo_processor_odom_3D.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/**
- * \file test_processor_odom_3D.cpp
- *
- *  Created on: Oct 7, 2016
- *      \author: jsola
- */
-
-#include "core/capture/capture_IMU.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_base.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-#include <cstdlib>
-
-using namespace wolf;
-using std::cout;
-using std::endl;
-using Eigen::Vector3s;
-using Eigen::Vector6s;
-using Eigen::Vector7s;
-using Eigen::Quaternions;
-using Eigen::VectorXs;
-
-int main (int argc, char** argv)
-{
-    cout << "\n========= Test ProcessorOdom3D ===========" << endl;
-
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    TimeStamp tf;
-    if (argc == 1)
-        tf = 1.0;
-    else
-    {
-
-        tf.set(strtod(argv[1],nullptr));
-    }
-    cout << "Final timestamp tf = " << tf.get() << " s" << endl;
-
-    ProblemPtr problem = Problem::create("PO", 3);
-    ceres::Solver::Options ceres_options;
-//    ceres_options.max_num_iterations = 1000;
-//    ceres_options.function_tolerance = 1e-10;
-//    ceres_options.gradient_check_relative_precision = 1e-10;
-//    ceres_options.gradient_tolerance = 1e-10;
-    ceres_options.minimizer_progress_to_stdout = true;
-    CeresManager ceres_manager(problem, ceres_options);
-
-    SensorBasePtr sen = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
-    ProcessorParamsOdom3DPtr proc_params = std::make_shared<ProcessorParamsOdom3D>();
-    problem->installProcessor("ODOM 3D", "odometry integrator", sen, proc_params);
-
-    // Time and prior
-    Scalar dt = 0.1;
-
-    problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity() * 1e-8, TimeStamp(0), dt/2);
-
-    // Motion data
-    Scalar dx = .1;
-    Scalar dyaw = 2*M_PI/5;
-    Vector6s data((Vector6s() << dx*cos(dyaw/2),dx*sin(dyaw/2),0,0,0,dyaw).finished()); // will integrate this data repeatedly
-
-    CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>("ODOM 3D", TimeStamp(0), sen, data, 7, 6, nullptr);
-
-    cout << "t: " << std::setprecision(2) << 0 << "  \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
-
-    for (TimeStamp t = dt; t < tf+dt/2; t += dt)
-    {
-        cap_odo->setTimeStamp(t);
-        cap_odo->setData(data);
-
-        sen->process(cap_odo);
-
-        cout << "t: " << std::setprecision(2) << t.get() << "  \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
-
-//        ceres::Solver::Summary summary = ceres_manager.solve();
-
-//        ceres_manager.computeCovariances(ALL);
-
-//        cout << summary.BriefReport() << endl;
-
-    }
-
-    problem->print(1,0,1,0);
-//    for (auto frm : problem->getTrajectory()->getFrameList())
-//    {
-//        frm->setState(problem->zeroState());
-//    }
-//    problem->print(1,0,1,0);
-    std::string brief_report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
-    std::cout << brief_report << std::endl;
-    problem->print(1,0,1,0);
-
-    problem.reset();
-
-    return 0;
-}
diff --git a/demos/demo_processor_tracker_feature.cpp b/demos/demo_processor_tracker_feature.cpp
deleted file mode 100644
index 74340d9e50d8fb1236f0db02c053f39f81807792..0000000000000000000000000000000000000000
--- a/demos/demo_processor_tracker_feature.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/**
- * \file test_processor_tracker_feature.cpp
- *
- *  Created on: Apr 11, 2016
- *      \author: jvallve
- */
-
-//std
-#include <iostream>
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_tracker_feature_dummy.h"
-#include "core/capture/capture_void.h"
-
-int main()
-{
-    using namespace wolf;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    std::cout << std::endl << "==================== processor tracker feature test ======================" << std::endl;
-
-    // Wolf problem
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
-    SensorBasePtr sensor_ptr_ = make_shared<SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
-                                             std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
-                                             std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
-    ProcessorParamsTrackerFeaturePtr params_trk = std::make_shared<ProcessorParamsTrackerFeature>();
-    params_trk->max_new_features = 4;
-    params_trk->min_features_for_keyframe = 7;
-    params_trk->time_tolerance = 0.25;
-    shared_ptr<ProcessorTrackerFeatureDummy> processor_ptr_ = make_shared<ProcessorTrackerFeatureDummy>(params_trk);
-
-    wolf_problem_ptr_->addSensor(sensor_ptr_);
-    sensor_ptr_->addProcessor(processor_ptr_);
-
-    std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
-    TimeStamp t(0);
-    Scalar dt = 0.5;
-    for (auto i = 0; i < 10; i++)
-    {
-        processor_ptr_->process(make_shared<CaptureVoid>(t, sensor_ptr_));
-        t += dt;
-    }
-
-    wolf_problem_ptr_->print(2);
-
-    return 0;
-}
-
diff --git a/demos/demo_processor_tracker_landmark.cpp b/demos/demo_processor_tracker_landmark.cpp
deleted file mode 100644
index 5500fdcbb126b413492620e7ea2828738a89300a..0000000000000000000000000000000000000000
--- a/demos/demo_processor_tracker_landmark.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/**
- * \file test_processor_tracker_landmark.cpp
- *
- *  Created on: Apr 12, 2016
- *      \author: jvallve
- */
-
-//std
-#include <iostream>
-
-//Wolf
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_tracker_landmark_dummy.h"
-#include "core/capture/capture_void.h"
-
-void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
-{
-    using namespace wolf;
-    std::cout << "\n-----------\nWolf tree begin" << std::endl;
-    std::cout << "Hrd: " << wolf_problem_ptr_->getHardware()->getProblem() << std::endl;
-    for (SensorBasePtr sen : wolf_problem_ptr_->getHardware()->getSensorList())
-    {
-        std::cout << "\tSen: " << sen->getProblem() << std::endl;
-        for (ProcessorBasePtr prc : sen->getProcessorList())
-        {
-            std::cout << "\t\tPrc: " << prc->getProblem() << std::endl;
-        }
-    }
-    std::cout << "Trj: " << wolf_problem_ptr_->getTrajectory()->getProblem() << std::endl;
-    for (FrameBasePtr frm : wolf_problem_ptr_->getTrajectory()->getFrameList())
-    {
-        std::cout << "\tFrm: " << frm->getProblem() << std::endl;
-        for (CaptureBasePtr cap : frm->getCaptureList())
-        {
-            std::cout << "\t\tCap: " << cap->getProblem() << std::endl;
-            for (FeatureBasePtr ftr : cap->getFeatureList())
-            {
-                std::cout << "\t\t\tFtr: " << ftr->getProblem() << std::endl;
-                for (FactorBasePtr ctr : ftr->getFactorList())
-                {
-                    std::cout << "\t\t\t\tCtr: " << ctr->getProblem() << std::endl;
-                }
-            }
-        }
-    }
-    std::cout << "Map: " << wolf_problem_ptr_->getMap()->getProblem() << std::endl;
-    for (LandmarkBasePtr lmk : wolf_problem_ptr_->getMap()->getLandmarkList())
-    {
-        std::cout << "\tLmk: " << lmk->getProblem() << std::endl;
-    }
-    std::cout << "Wolf tree end\n-----------\n" << std::endl;
-}
-
-int main()
-{
-    using namespace wolf;
-
-    std::cout << std::endl << "==================== processor tracker landmark test ======================" << std::endl;
-
-    // Wolf problem
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
-    // SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
-    //                                          std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
-    //                                          std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-    auto sensor_ptr_ = SensorBase::emplace<SensorBase>(wolf_problem_ptr_->getHardware(), "ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
-                                                       std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
-                                                       std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-    ProcessorParamsTrackerLandmarkPtr params_trk = std::make_shared<ProcessorParamsTrackerLandmark>();
-    params_trk->max_new_features = 5;
-    params_trk->min_features_for_keyframe = 7;
-    params_trk->time_tolerance = 0.25;
-    // std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk);
-    std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ =
-        std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(ProcessorBase::emplace<ProcessorTrackerLandmarkDummy>(sensor_ptr_, params_trk));
-    // wolf_problem_ptr_->addSensor(sensor_ptr_);
-    // sensor_ptr_->addProcessor(processor_ptr_);
-
-    std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
-    TimeStamp t(0);
-    Scalar dt = 0.5;
-    for (auto i = 0; i < 10; i++)
-    {
-        processor_ptr_->process(std::make_shared<CaptureVoid>(t, sensor_ptr_));
-        t += dt;
-    }
-
-    wolf_problem_ptr_->print(2);
-
-    return 0;
-}
-
diff --git a/demos/demo_sh_ptr.cpp b/demos/demo_sh_ptr.cpp
deleted file mode 100644
index 3a10903c85ee72967dbe8ca23dd1584dbda494b8..0000000000000000000000000000000000000000
--- a/demos/demo_sh_ptr.cpp
+++ /dev/null
@@ -1,795 +0,0 @@
-/**
- * \file test_sh_ptr.cpp
- *
- *  Created on: Oct 4, 2016
- *      \author: jsola
- *
- *  Complete Wolf tree skeleton with smart pointers
- *
- */
-
-// C++11
-#include <memory>
-
-using std::shared_ptr;
-using std::weak_ptr;
-using std::make_shared;
-using std::enable_shared_from_this;
-
-// std
-#include <list>
-#include <vector>
-#include <iostream>
-using std::list;
-using std::vector;
-using std::cout;
-using std::endl;
-
-namespace wolf
-{
-// fwds
-class H; // hardware
-class S; // sensor
-class p; // processor
-class T; // trajectory
-class F; // frame
-class C; // capture
-class f; // feature
-class c; // factor
-class M; // map
-class L; // landmark
-
-//////////////////////////////////////////////////////////////////////////////////
-// DECLARE WOLF TREE
-
-class P : public enable_shared_from_this<P>
-{
-    private:
-        shared_ptr<H> H_ptr_;
-        shared_ptr<T> T_ptr_;
-        shared_ptr<M> M_ptr_;
-
-    public:
-        P();
-        ~P(){cout << "destruct P" << endl;}
-        void setup();
-        shared_ptr<H> getH(){return H_ptr_;}
-        shared_ptr<T> getT(){return T_ptr_;}
-        shared_ptr<M> getM(){return M_ptr_;}
-};
-
-class H : public enable_shared_from_this<H>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        list<shared_ptr<S>> S_list_;
-
-    public:
-        H(){cout << "construct H" << endl;}
-        ~H(){cout << "destruct H" << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P> _P){P_ptr_ = _P;}
-        list<shared_ptr<S>>& getSlist() {return S_list_;}
-        shared_ptr<S> add_S(shared_ptr<S> _S);
-};
-
-class S : public enable_shared_from_this<S>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<H> H_ptr_;
-        list<shared_ptr<p>> p_list_;
-        static int id_count_;
-
-        //        list<shared_ptr<C>> C_list_; // List of all captures
-
-    public:
-        int id;
-        S():id(++id_count_){cout << "construct + S" << id << endl;}
-        ~S(){cout << "destruct + S" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-             if (!P_sh)
-                 P_sh = getH()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<H> getH(){
-            shared_ptr<H> H_sh = H_ptr_.lock();
-            return H_sh;
-        }
-        void setH(const shared_ptr<H> _H){H_ptr_ = _H;}
-        list<shared_ptr<p>>& getplist() {return p_list_;}
-        shared_ptr<p> add_p(shared_ptr<p> _p);
-};
-
-class p : public enable_shared_from_this<p>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<S> S_ptr_;
-        static int id_count_;
-
-    public:
-        int id;
-        p():id(++id_count_){cout << "construct   + p" << id << endl;}
-        ~p(){cout << "destruct   + p" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            if (!P_sh)
-                P_sh = getS()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<S> getS(){
-            shared_ptr<S> S_sh = S_ptr_.lock();
-            return S_sh;
-        }
-        void setS(const shared_ptr<S> _S){S_ptr_ = _S;}
-};
-
-class T : public enable_shared_from_this<T>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        list<shared_ptr<F>> F_list_;
-
-    public:
-        T(){cout << "construct T" << endl;}
-        ~T(){cout << "destruct T" << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P> _P){P_ptr_ = _P;}
-        list<shared_ptr<F>>& getFlist() {return F_list_;}
-        shared_ptr<F> add_F(shared_ptr<F> _F);
-};
-
-class F : public enable_shared_from_this<F>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<T> T_ptr_;
-        list<shared_ptr<C>> C_list_;
-
-        list<shared_ptr<c>> c_by_list; // list of factors to this frame
-
-        static int id_count_;
-        bool is_removing;
-
-    public:
-        int id;
-        F() :is_removing(false),id(++id_count_){cout << "construct + F" << id << endl;}
-        ~F(){cout << "destruct + F" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            if (!P_sh)
-                P_sh = getT()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<T> getT(){
-            shared_ptr<T> T_sh = T_ptr_.lock();
-            return T_sh;
-        }
-        void setT(const shared_ptr<T> _T){T_ptr_ = _T;}
-        list<shared_ptr<C>>& getClist() {return C_list_;}
-        list<shared_ptr<c>>& getCbyList(){return c_by_list;}
-        shared_ptr<C> add_C(shared_ptr<C> _C);
-        void add_c_by(shared_ptr<c> _cby)
-        {
-            c_by_list.push_back(_cby);
-        }
-        void remove();
-
-};
-
-class C : public enable_shared_from_this<C>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<F> F_ptr_;
-        list<shared_ptr<f>> f_list_;
-
-        weak_ptr<S> S_ptr_; // sensor
-        static int id_count_;
-        bool is_deleting;
-
-    public:
-        int id;
-        C():is_deleting(false),id(++id_count_){cout << "construct   + C" << id << endl;}
-        ~C(){cout << "destruct   + C" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            if (!P_sh)
-                P_sh = getF()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<F> getF(){
-            shared_ptr<F> F_sh = F_ptr_.lock();
-            return F_sh;
-        }
-        void setF(const shared_ptr<F> _F){F_ptr_ = _F;}
-        list<shared_ptr<f>>& getflist() {return f_list_;}
-        shared_ptr<f> add_f(shared_ptr<f> _f);
-        void remove();
-
-};
-
-class f : public enable_shared_from_this<f>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<C> C_ptr_;
-        list<shared_ptr<c>> c_list_;
-
-        list<shared_ptr<c>> c_by_list; // list of factors to this feature
-
-        static int id_count_;
-        bool is_deleting;
-
-    public:
-        int id;
-        f():is_deleting(false),id(++id_count_){cout << "construct     + f" << id << endl;}
-        ~f(){cout << "destruct     + f" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            if (!P_sh)
-                P_sh = getC()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<C> getC(){
-            shared_ptr<C> C_sh = C_ptr_.lock();
-            return C_sh;
-        }
-        void setC(const shared_ptr<C> _C){C_ptr_ = _C;}
-        list<shared_ptr<c>>& getclist() {return c_list_;}
-        list<shared_ptr<c>>& getCbyList(){return c_by_list;}
-        shared_ptr<c> add_c(shared_ptr<c> _c);
-        void add_c_by(shared_ptr<c> _cby)
-        {
-            c_by_list.push_back(_cby);
-        }
-        void remove();
-};
-
-class c : public enable_shared_from_this<c>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<f> f_ptr_; // change this to shared??
-
-        // can we have just one pointer? Derive 3 classes from c?
-        weak_ptr<F> F_other_ptr_; // change this to shared?
-        weak_ptr<f> f_other_ptr_; // change this to shared?
-        weak_ptr<L> L_other_ptr_; // change this to shared?
-
-        static int id_count_;
-        bool is_deleting;
-
-    public:
-        int id;
-        enum{c0, cF, cf, cL} type;
-        c():is_deleting(false),id(++id_count_){type = c0; cout << "construct       + c" << id << endl;}
-        c(shared_ptr<F> _F_other);
-        c(shared_ptr<f> _f_other);
-        c(shared_ptr<L> _L_other);
-        ~c(){cout << "destruct       + c" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            if (!P_sh)
-                P_sh = getf()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<f> getf(){
-            shared_ptr<f> f_sh = f_ptr_.lock();
-            return f_sh;
-        }
-        void setf(const shared_ptr<f> _f){f_ptr_ = _f;}
-        shared_ptr<F> getFother(){return F_other_ptr_.lock();}
-        shared_ptr<f> getfother(){return f_other_ptr_.lock();}
-        shared_ptr<L> getLother(){return L_other_ptr_.lock();}
-        void remove();
-};
-
-class M : public enable_shared_from_this<M>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        list<shared_ptr<L>> L_list_;
-
-    public:
-        M(){cout << "construct M" << endl;}
-        ~M(){cout << "destruct M" << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P> _P){P_ptr_ = _P;}
-        list<shared_ptr<L>>& getLlist() {return L_list_;}
-        shared_ptr<L> add_L(shared_ptr<L> _L);
-};
-
-class L : public enable_shared_from_this<L>
-{
-    private:
-        weak_ptr<P> P_ptr_;
-        weak_ptr<M> M_ptr_;
-
-        list<shared_ptr<c>> c_by_list; // list of factors to this landmark
-
-        static int id_count_;
-        bool is_deleting;
-
-    public:
-        int id;
-        L():is_deleting(false),id(++id_count_){cout << "construct + L" << id << endl;}
-        ~L(){cout << "destruct + L" << id << endl;}
-        shared_ptr<P> getP(){
-            shared_ptr<P> P_sh = P_ptr_.lock();
-            if (!P_sh)
-                P_sh = getM()->getP();
-            return P_sh;
-        }
-        void setP(const shared_ptr<P>& _P){P_ptr_ = _P;}
-        shared_ptr<M> getM(){
-            shared_ptr<M> M_sh = M_ptr_.lock();
-            return M_sh;
-        }
-        void setM(const shared_ptr<M> _M){M_ptr_ = _M;}
-        list<shared_ptr<c>>& getCbyList(){return c_by_list;}
-        void add_c_by(shared_ptr<c> _cby)
-        {
-            c_by_list.push_back(_cby);
-        }
-        void remove();
-};
-
-//////////////////////////////////////////////////////////////////////////////////
-// IMPLEMENTATION of some methods
-
-P::P(){
-    cout << "construct P" << endl;
-    H_ptr_ = make_shared<H>();
-    T_ptr_ = make_shared<T>();
-    M_ptr_ = make_shared<M>();
-    cout << "P is constructed" << endl;
-}
-
-void P::setup()
-{
-    H_ptr_->setP(shared_from_this());
-    T_ptr_->setP(shared_from_this());
-    M_ptr_->setP(shared_from_this());
-    cout << "P is set up" << endl;
-}
-
-shared_ptr<S> H::add_S(shared_ptr<S> _S)
-{
-    S_list_.push_back(_S);
-    _S->setH(shared_from_this());
-    _S->setP(getP());
-    return _S;
-}
-
-shared_ptr<p> S::add_p(shared_ptr<p> _p)
-{
-    p_list_.push_back(_p);
-    _p->setS(shared_from_this());
-    _p->setP(getP());
-    return _p;
-}
-
-shared_ptr<F> T::add_F(shared_ptr<F> _F)
-{
-    F_list_.push_back(_F);
-    _F->setT(shared_from_this());
-    _F->setP(getP());
-    return _F;
-}
-
-shared_ptr<C> F::add_C(shared_ptr<C> _C)
-{
-    C_list_.push_back(_C);
-    _C->setF(shared_from_this());
-    _C->setP(getP());
-    return _C;
-}
-void F::remove()
-{
-    if (!is_removing)
-    {
-        is_removing = true;
-        cout << "Removing   F" << id << endl;
-        shared_ptr<F> this_F = shared_from_this();  // keep this alive while removing it
-        getT()->getFlist().remove(this_F);          // remove from upstream
-        while (!C_list_.empty())
-            C_list_.front()->remove();          // remove downstream
-        while (!c_by_list.empty())
-            c_by_list.front()->remove();        // remove constrained
-    }
-}
-
-shared_ptr<f> C::add_f(shared_ptr<f> _f)
-{
-    f_list_.push_back(_f);
-    _f->setC(shared_from_this());
-    _f->setP(getP());
-    return _f;
-}
-void C::remove()
-{
-    if (!is_deleting)
-    {
-        is_deleting = true;
-        cout << "Removing     C" << id << endl;
-        shared_ptr<C> this_C = shared_from_this();  // keep this alive while removing it
-        getF()->getClist().remove(this_C);          // remove from upstream
-        if (getF()->getClist().empty() && getF()->getCbyList().empty())
-            getF()->remove();                   // remove upstream
-        while (!f_list_.empty())
-            f_list_.front()->remove();          // remove downstream
-    }
-}
-
-shared_ptr<c> f::add_c(shared_ptr<c> _c)
-{
-    c_list_.push_back(_c);
-    _c->setf(shared_from_this());
-    _c->setP(getP());
-    return _c;
-}
-void f::remove()
-{
-    if (!is_deleting)
-    {
-        is_deleting = true;
-        cout << "Removing       f" << id << endl;
-        shared_ptr<f> this_f = shared_from_this();  // keep this alive while removing it
-        getC()->getflist().remove(this_f);          // remove from upstream
-        if (getC()->getflist().empty())
-            getC()->remove();                   // remove upstream
-        while (!c_list_.empty())
-            c_list_.front()->remove();          // remove downstream
-        while (!c_by_list.empty())
-            c_by_list.front()->remove();        // remove constrained
-    }
-}
-
-c::c(shared_ptr<F> _F_other):is_deleting(false),id(++id_count_)
-{
-    cout << "construct       + c" << id << " -> F" << _F_other->id << endl;
-    type = cF;
-    F_other_ptr_ = _F_other;
-}
-
-c::c(shared_ptr<f> _f_other):is_deleting(false),id(++id_count_)
-{
-    cout << "construct       + c" << id << " -> f" << _f_other->id << endl;
-    type = cf;
-    f_other_ptr_ = _f_other;
-}
-
-c::c(shared_ptr<L> _L_other):is_deleting(false),id(++id_count_)
-{
-    cout << "construct       + c" << id << " -> L" << _L_other->id << endl;
-    type = cL;
-    L_other_ptr_ = _L_other;
-}
-
-void c::remove()
-{
-    if (!is_deleting)
-    {
-        is_deleting = true;
-        cout << "Removing         c" << id << endl;
-        shared_ptr<c> this_c = shared_from_this();  // keep this alive while removing it
-        getf()->getclist().remove(this_c);          // remove from upstream
-        if (getf()->getclist().empty() && getf()->getCbyList().empty())
-            getf()->remove();                   // remove upstream
-
-        // remove other: {Frame, feature, Landmark}
-        switch (type)
-        {
-            case c::cF:
-                getFother()->getCbyList().remove(this_c);
-                if (getFother()->getCbyList().empty() && getFother()->getClist().empty())
-                    getFother()->remove();
-                break;
-            case c::cf:
-                getfother()->getCbyList().remove(this_c);
-                if (getfother()->getCbyList().empty() && getfother()->getclist().empty())
-                    getfother()->remove();
-                break;
-            case c::cL:
-                getLother()->getCbyList().remove(this_c);
-                if (getLother()->getCbyList().empty())
-                    getLother()->remove();
-                break;
-            default:
-                break;
-        }
-    }
-}
-
-shared_ptr<L> M::add_L(shared_ptr<L> _L)
-{
-    L_list_.push_back(_L);
-    _L->setM(shared_from_this());
-    _L->setP(getP());
-    return _L;
-}
-
-void L::remove()
-{
-    if (!is_deleting)
-    {
-        is_deleting = true;
-        cout << "Removing   L" << id << endl;
-        shared_ptr<L> this_L = shared_from_this();  // keep this alive while removing it
-        getM()->getLlist().remove(this_L);          // remove from upstream
-        while (!c_by_list.empty())
-            c_by_list.front()->remove();        // remove constrained
-    }
-}
-
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// HELPERS
-
-using namespace wolf;
-
-void print_cF(const shared_ptr<P>& Pp)
-{
-    cout << "Frame factors" << endl;
-    for (auto Fp : Pp->getT()->getFlist())
-    {
-        cout << "F" << Fp->id << " @ " << Fp.get() << endl;
-        for (auto cp : Fp->getCbyList())
-        {
-            cout << " -> c" << cp->id << " @ " << cp.get()
-                    << " -> F" << cp->getFother()->id << " @ " << cp->getFother().get() << endl;
-        }
-    }
-}
-
-void print_cf(const shared_ptr<P>& Pp)
-{
-    cout << "Feature factors" << endl;
-    for (auto Fp : Pp->getT()->getFlist())
-    {
-        for (auto Cp : Fp->getClist())
-        {
-            for (auto fp : Cp->getflist())
-            {
-                cout << "f" << fp->id << " @ " << fp.get() << endl;
-                for (auto cp : fp->getCbyList())
-                {
-                cout << " -> c" << cp->id << " @ " << cp.get()
-                        << " -> f" << cp->getfother()->id << " @ " << cp->getfother().get() << endl;
-                }
-            }
-        }
-    }
-}
-
-void print_cL(const shared_ptr<P>& Pp)
-{
-    cout << "Landmark factors" << endl;
-    for (auto Lp : Pp->getM()->getLlist())
-    {
-        cout << "L" << Lp->id << " @ " << Lp.get() << endl;
-        for (auto cp : Lp->getCbyList())
-        {
-            cout << " -> c" << cp->id << " @ " << cp.get()
-                    << " -> L" << cp->getLother()->id << " @ " << cp->getLother().get() << endl;
-        }
-    }
-}
-
-void print_c(const shared_ptr<P>& Pp)
-{
-    cout << "All factors" << endl;
-    for (auto Fp : Pp->getT()->getFlist())
-    {
-        for (auto Cp : Fp->getClist())
-        {
-            for (auto fp : Cp->getflist())
-            {
-                for (auto cp : fp->getclist())
-                {
-                    if (cp)
-                    {
-                        switch (cp->type)
-                        {
-                            case c::cF:
-                                cout << "c" << cp->id << " @ " << cp.get()
-                                << " -> F" << cp->getFother()->id << " @ " << cp->getFother() << endl;
-                                break;
-                            case c::cf:
-                                cout << "c" << cp->id << " @ " << cp.get()
-                                << " -> f" << cp->getfother()->id << " @ " << cp->getfother() << endl;
-                                break;
-                            case c::cL:
-                                cout << "c" << cp->id << " @ " << cp.get()
-                                << " -> L" << cp->getLother()->id << " @ " << cp->getLother() << endl;
-                                break;
-                            default:
-                                cout << "Bad factor" << endl;
-                                break;
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
-
-/** Build problem
- * See the problem built here:
- *   https://docs.google.com/drawings/d/1vYmhBjJz7AHxOdy0gV-77hqsOYfGVuvUmnRVB-Zfp_Q/edit
- */
-shared_ptr<P> buildProblem(int N)
-{
-    shared_ptr<P> Pp = make_shared<P>();
-    Pp->setup();
-    // H
-    for (int Si = 0; Si < 2; Si++)
-    {
-        shared_ptr<S> Sp = Pp->getH()->add_S(make_shared<S>());
-        for (int pi = 0; pi < 2; pi++)
-        {
-            shared_ptr<p> pp = Sp->add_p(make_shared<p>());
-        }
-    }
-    // M
-    for (int Li = 0; Li < 2; Li++)
-    {
-        shared_ptr<L> Lp = Pp->getM()->add_L(make_shared<L>());
-    }
-    // T
-    list<shared_ptr<L> >::iterator Lit = Pp->getM()->getLlist().begin();
-    vector<weak_ptr<F> > Fvec(N);
-    for (int Fi = 0; Fi < N; Fi++)
-    {
-        shared_ptr<F> Fp = Pp->getT()->add_F(make_shared<F>());
-        Fvec.at(Fi) = Fp;
-        for (int Ci = 0; Ci < 2; Ci++)
-        {
-            shared_ptr<C> Cp = Fp->add_C(make_shared<C>());
-            for (int fi = 0; fi < 1; fi++)
-            {
-                shared_ptr<f> fp = Cp->add_f(make_shared<f>());
-                if (Ci || !Fi) // landmark factor
-                {
-                    shared_ptr<c> cp = fp->add_c(make_shared<c>(*Lit));
-                    (*Lit)->add_c_by(cp);
-                    Lit++;
-                    if (Lit == Pp->getM()->getLlist().end())
-                        Lit = Pp->getM()->getLlist().begin();
-                }
-                else // motion factor
-                {
-                    shared_ptr<F> Fp = Fvec.at(Fi-1).lock();
-                    if (Fp)
-                    {
-                        shared_ptr<c> cp = fp->add_c(make_shared<c>(Fp));
-                        Fp->add_c_by(cp);
-                    }
-                    else
-                        cout << "Could not constrain Frame" << endl;
-                }
-            }
-        }
-    }
-    return Pp;
-}
-
-// init ID factories
-int S::id_count_ = 0;
-int p::id_count_ = 0;
-int F::id_count_ = 0;
-int C::id_count_ = 0;
-int f::id_count_ = 0;
-int c::id_count_ = 0;
-int L::id_count_ = 0;
-
-// tests
-void removeFactors(const shared_ptr<P>& Pp)
-{
-    cout << "Removing factor type L ----------" << endl;
-    Pp->getT()->getFlist().front()->getClist().front()->getflist().front()->getclist().front()->remove();
-    cout << "Removing factor type L ----------" << endl;
-    Pp->getT()->getFlist().front()->getClist().front()->getflist().front()->getclist().front()->remove();
-    cout << "Removing factor type F ----------" << endl;
-    Pp->getT()->getFlist().back()->getClist().front()->getflist().front()->getclist().front()->remove();
-    cout << "Removing factor type L ----------" << endl;
-    Pp->getT()->getFlist().back()->getClist().back()->getflist().front()->getclist().front()->remove();
-    cout << "Removing factor type F ----------" << endl;
-    Pp->getT()->getFlist().back()->getClist().front()->getflist().front()->getclist().front()->remove();
-}
-
-void removeLandmarks(const shared_ptr<P>& Pp)
-{
-    cout << "Removing landmark ----------" << endl;
-    Pp->getM()->getLlist().front()->remove();
-    cout << "Removing landmark ----------" << endl;
-    Pp->getM()->getLlist().front()->remove();
-}
-
-void removeFrames(const shared_ptr<P>& Pp)
-{
-    cout << "Removing frame ----------" << endl;
-    Pp->getT()->getFlist().back()->remove();
-    cout << "Removing frame ----------" << endl;
-    Pp->getT()->getFlist().front()->remove();
-    cout << "Removing frame ----------" << endl;
-    Pp->getT()->getFlist().back()->remove();
-}
-
-void removeLmksAndFrames(const shared_ptr<P>& Pp)
-{
-    cout << "Removing landmark ----------" << endl;
-    Pp->getM()->getLlist().front()->remove();
-    cout << "Removing frame ----------" << endl;
-    Pp->getT()->getFlist().front()->remove();
-    cout << "Removing frame ----------" << endl;
-    Pp->getT()->getFlist().back()->remove();
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// MAIN
-int main()
-{
-    int N = 3;
-
-    shared_ptr<P> Pp = buildProblem(N);
-    cout << "Wolf tree created ----------------------------" << endl;
-
-    cout << "\nShowing factors --------------------------" << endl;
-    cout<<endl;
-    print_cF(Pp);
-    cout<<endl;
-    print_cf(Pp);
-    cout<<endl;
-    print_cL(Pp);
-    cout<<endl;
-    print_c(Pp);
-
-    //------------------------------------------------------------------
-    // Several tests. Uncomment the desired test.
-    // Run only one test at a time, otherwise you'll get segfaults!
-
-//    cout << "\nRemoving factors -------------------------" << endl;
-//    removeFactors(Pp);
-
-//    cout << "\nRemoving landmarks ---------------------------" << endl;
-//    removeLandmarks(Pp);
-
-//    cout << "\nRemoving frames ------------------------------" << endl;
-//    removeFrames(Pp);
-
-    cout << "\nRemoving lmks and frames ------------------------------" << endl;
-    removeLmksAndFrames(Pp);
-
-//    cout << "\nRemoving problem ---------------------------" << endl;
-//    Pp.reset();
-
-//    cout << "\nRebuilding problem ---------------------------" << endl;
-//    Pp = buildProblem(N);
-
-    //------------------------------------------------------------------
-
-    cout << "\nExiting main() -------------------------------" << endl;
-
-    return 1;
-}
-
diff --git a/demos/demo_sort_keyframes.cpp b/demos/demo_sort_keyframes.cpp
deleted file mode 100644
index a1f225eddd560462b6af13e7898990cdaea6d4dc..0000000000000000000000000000000000000000
--- a/demos/demo_sort_keyframes.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * \file test_sort_keyframes.cpp
- *
- *  Created on: May 24, 2016
- *      \author: jvallve
- */
-
-// Wolf includes
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/frame/frame_base.h"
-#include "core/trajectory/trajectory_base.h"
-
-// STL includes
-#include <list>
-#include <memory>
-
-// General includes
-#include <iostream>
-
-using namespace wolf;
-
-void printFrames(ProblemPtr _problem_ptr)
-{
-    std::cout << "TRAJECTORY:" << std::endl;
-    for (auto frm : _problem_ptr->getTrajectory()->getFrameList())
-        std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl;
-}
-
-int main()
-{
-    ProblemPtr problem_ptr = Problem::create(FRM_PO_2D);
-
-    problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
-    FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
-    FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
-    problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
-    FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
-    FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
-
-    printFrames(problem_ptr);
-
-    std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl;
-    frm5->setEstimated();
-
-    printFrames(problem_ptr);
-
-    std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl;
-    frm2->setEstimated();
-
-    printFrames(problem_ptr);
-
-    std::cout << std::endl << "Frame " << frm3->id() << " new TimeStamp:" << 0.45 << std::endl;
-    frm3->setTimeStamp(TimeStamp(0.45));
-
-    printFrames(problem_ptr);
-
-    std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl;
-    frm3->setEstimated();
-
-    printFrames(problem_ptr);
-
-    std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl;
-    frm6->setEstimated();
-
-    printFrames(problem_ptr);
-
-    FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
-    std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
-
-    printFrames(problem_ptr);
-
-    FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
-    std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
-
-    printFrames(problem_ptr);
-
-    return 0;
-}
diff --git a/demos/demo_sparsification.cpp b/demos/demo_sparsification.cpp
deleted file mode 100644
index a521cabd1b5b1276846573df70b53d2b70b6d537..0000000000000000000000000000000000000000
--- a/demos/demo_sparsification.cpp
+++ /dev/null
@@ -1,314 +0,0 @@
-// Sparsification example creating wolf tree from imported graph from .txt file
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//std includes
-#include <cstdlib>
-#include <fstream>
-#include <string>
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "core/capture/capture_void.h"
-#include "core/feature/feature_odom_2D.h"
-#include "core/factor/factor_base.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// EIGEN
-//#include <Eigen/CholmodSupport>
-#include <Eigen/StdVector> // Eigen in std vector
-
-namespace wolf{
-// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-
-void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (int k=0; k<ins.outerSize(); ++k)
-    for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
-      original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
-
-  original.makeCompressed();
-}
-
-void decodeEdge(const std::string& buffer, unsigned int& edge_from, unsigned int& edge_to, Eigen::Vector3s& measurement, Eigen::Matrix3s& covariance)
-{
-	std::string str_num;
-
-	unsigned int i = 0;
-
-	// only decode edges
-	if (buffer.at(0) == 'E')
-	{
-		//skip rest of EDGE word
-		while (buffer.at(i) != ' ') i++;
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-
-		// FROM ID
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		edge_from = atoi(str_num.c_str())+1;
-		str_num.clear();
-
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-
-		// TO ID
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		edge_to = atoi(str_num.c_str())+1;
-		str_num.clear();
-
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-
-		// MEASUREMENT
-		// X
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		measurement(0) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// Y
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		measurement(1) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// THETA
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		measurement(2) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-
-		// INFORMATION
-		Eigen::Matrix3s information;
-		// XX
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		information(0,0) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// XY
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		information(0,1) = atof(str_num.c_str());
-		information(1,0) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// YY
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		information(1,1) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// THETATHETA
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		information(2,2) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// XTHETA
-		while (buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		information(0,2) = atof(str_num.c_str());
-		information(2,0) = atof(str_num.c_str());
-		str_num.clear();
-		//skip white spaces
-		while (buffer.at(i) == ' ') i++;
-		// YTHETA
-		while (i < buffer.size() && buffer.at(i) != ' ')
-			str_num.push_back(buffer.at(i++));
-		information(1,2) = atof(str_num.c_str());
-		information(2,1) = atof(str_num.c_str());
-		str_num.clear();
-
-		// COVARIANCE
-		covariance = information.inverse();
-	}
-	else
-	{
-		edge_from = 0;
-		edge_to   = 0;
-	}
-}
-
-}
-
-int main(int argc, char** argv) 
-{
-    using namespace wolf;
-
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-
-    bool wrong_input = false;
-    if (argc < 3)
-    	wrong_input = true;
-    else if (argc > 4)
-    	wrong_input = true;
-    else if (argc > 2 && (atoi(argv[2]) < 2 || atoi(argv[2]) > 5))
-    	wrong_input = true;
-    else if (argc > 3 && atoi(argv[3]) < 0 )
-    	wrong_input = true;
-
-    if (wrong_input)
-    {
-        std::cout << "Please call me with: [./test_wolf_imported_graph DATASET T (MAX_VERTICES)], where:" << std::endl;
-        std::cout << "    DATASET: manhattan, killian or intel" << std::endl;
-        std::cout << "    T keep one node each T: 2, 3, 4 or 5" << std::endl;
-        std::cout << "    optional: MAX_VERTICES max edges to be loaded" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    // input variables
-    char const * dataset_path = std::getenv("DATASET_PATH");
-	unsigned int pruning_T = atoi(argv[2]);
-    std::string file_path(dataset_path);
-	file_path = file_path + "/graphs/redirected_" + std::to_string(pruning_T) + "_" + argv[1] + ".graph";
-	unsigned int MAX_VERTEX = 1e9;
-	if (argc > 3 && atoi(argv[3]) != 0)
-    	MAX_VERTEX = atoi(argv[3]);
-
-    // Wolf problem
-    FrameBasePtr last_frame_ptr, frame_from_ptr, frame_to_ptr;
-    ProblemPtr bl_problem_ptr = Problem::create("PO_2D");
-    SensorBasePtr sensor_ptr = bl_problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBasePtr());
-
-    // Ceres wrapper
-    std::string bl_summary, sp_summary;
-    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 = 10;
-    CeresManagerPtr bl_ceres_manager = std::make_shared<CeresManager>(bl_problem_ptr, ceres_options);
-
-    // load graph from .txt
-    std::ifstream graph_file;
-    graph_file.open(file_path.c_str(), std::ifstream::in);
-    if (!graph_file.is_open())
-    {
-    	printf("\nError opening file: %s\n",file_path.c_str());
-    	return -1;
-    }
-
-    // auxiliar variables
-	std::string line_string;
-	unsigned int edge_from, edge_to;
-	Eigen::Vector3s meas;
-	Eigen::Matrix3s meas_cov;
-	Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
-	//clock_t t1;
-
-	// ------------------------ START EXPERIMENT ------------------------
-	// First frame FIXED
-	last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0));
-	last_frame_ptr->fix();
-	bl_problem_ptr->print(4, true, false, true);
-
-	while (std::getline(graph_file, line_string) && last_frame_ptr->id() <= MAX_VERTEX)
-	{
-		std::cout << "new line:" << line_string << std::endl;
-		decodeEdge(line_string, edge_from, edge_to, meas, meas_cov);
-
-		// only factors
-		if (edge_from != 0)
-		{
-
-			// ODOMETRY -------------------
-			if (edge_to > last_frame_ptr->id())
-			{
-				frame_from_ptr = last_frame_ptr;
-
-				// NEW KEYFRAME
-				Eigen::Vector3s from_pose = frame_from_ptr->getState();
-				R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix();
-				Eigen::Vector3s new_frame_pose = from_pose + R*meas;
-				last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
-
-				frame_to_ptr = last_frame_ptr;
-
-				std::cout << "NEW FRAME " << last_frame_ptr->id() << " - ts = " << last_frame_ptr->getTimeStamp().get() << std::endl;
-
-				// REMOVE PREVIOUS NODES
-			}
-			// LOOP CLOSURE ---------------
-			else
-			{
-				if (edge_from == last_frame_ptr->id())
-					frame_from_ptr = last_frame_ptr;
-				else
-					for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
-						if ((*frm_rit)->id() == edge_from)
-						{
-							frame_from_ptr = *frm_rit;
-							break;
-						}
-				if (edge_to == last_frame_ptr->id())
-					frame_to_ptr = last_frame_ptr;
-				else
-					for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
-						if ((*frm_rit)->id() == edge_to)
-						{
-							frame_to_ptr = *frm_rit;
-							break;
-						}
-			}
-//			std::cout << "frame_from " << frame_from_ptr->id() << std::endl;
-//			std::cout << "edge_from " << edge_from << std::endl;
-//			std::cout << "frame_to " << frame_to_ptr->id() << std::endl;
-//			std::cout << "edge_to " << edge_to << std::endl;
-
-			assert(frame_from_ptr->id() == edge_from && "frame from id and edge from idx must be the same");
-			assert(frame_to_ptr->id() == edge_to && "frame to id and edge to idx must be the same");
-
-			// CAPTURE
-			CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
-			frame_from_ptr->addCapture(capture_ptr);
-
-			// FEATURE
-			FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(meas, meas_cov);
-			capture_ptr->addFeature(feature_ptr);
-
-			// CONSTRAINT
-			FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, frame_to_ptr);
-			feature_ptr->addFactor(factor_ptr);
-			frame_to_ptr->addConstrainedBy(factor_ptr);
-
-			// SOLVE
-			// solution
-      bl_summary = bl_ceres_manager->solve(SolverManager::ReportVerbosity::FULL);
-		    std::cout << bl_summary << std::endl;
-
-			// covariance
-        bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS
-
-	//		t1 = clock();
-	//		double t_sigma_manual = 0;
-	//		t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-		}
-	}
-
-	//bl_problem_ptr->print(4, true, false, true);
-
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
diff --git a/demos/demo_state_quaternion.cpp b/demos/demo_state_quaternion.cpp
deleted file mode 100644
index 6528a2b3935e488c7534c44f031a7cff22a112ed..0000000000000000000000000000000000000000
--- a/demos/demo_state_quaternion.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * \file test_state_quaternion.cpp
- *
- *  Created on: Mar 7, 2016
- *      \author: jsola
- */
-
-#include "core/frame/frame_base.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/common/time_stamp.h"
-
-#include <iostream>
-
-int main (void)
-{
-    using namespace wolf;
-
-    // 3D
-    StateBlockPtr pp = std::make_shared<StateBlock>(3);
-    StateQuaternionPtr op = std::make_shared<StateQuaternion>();
-    StateBlockPtr vp = std::make_shared<StateBlock>(3);
-
-    TimeStamp t;
-
-    FrameBase pqv(t,pp,op,vp);
-
-    std::cout << "P local param: " << pqv.getP()->getLocalParametrization() << std::endl;
-    std::cout << "Q local param: " << pqv.getO()->getLocalParametrization() << std::endl;
-    std::cout << "V local param: " << pqv.getV()->getLocalParametrization() << std::endl;
-
-    //    delete pp;
-    //    delete op;
-    //    delete vp;
-    // Note: Deleting the StateBlock pointers will be done at the destruction of FrameBase.
-
-    std::cout << "Done" << std::endl;
-
-    return 1;
-}
diff --git a/demos/demo_virtual_hierarchy.cpp b/demos/demo_virtual_hierarchy.cpp
deleted file mode 100644
index fb1b248ce8bae0371579391351daec340fa2c8c5..0000000000000000000000000000000000000000
--- a/demos/demo_virtual_hierarchy.cpp
+++ /dev/null
@@ -1,354 +0,0 @@
-/*
- * test_virtual_hierarchy.cpp
- *
- *  Created on: Sep 8, 2014
- *      Author: jsola
- */
-
-#include <list>
-
-namespace wolf{
-
-// BASE CLASSES
-
-/**
- * \brief Node class.
- *
- * The Node class has only an ID and an ID factory built in the constructor.
- */
-class N
-{
-    private:
-        unsigned int id_;
-        static unsigned int id_count_;
-    protected:
-        N() : id_(++id_count_) { }
-        virtual ~N() { }
-    public:
-        unsigned int id() { return id_; }
-};
-
-/**
- * \brief Base class for children.
- *
- * It has a pointer to parent.
- *
- * NOTE: The virtual inheritance solves the "diamond of death" problem.
- */
-template<class Parent>
-class ChildOf : virtual public N
-{
-    private:
-        Parent* up_ptr_;
-    protected:
-        ChildOf(Parent* _up_ptr) : up_ptr_(_up_ptr) { }
-        virtual ~ChildOf() { }
-        Parent* up() { return up_ptr_; }
-};
-
-/**
- * \brief Base class for parents
- *
- * It has a list of pointers to children, and a dumy method 'print' that is recursive to all children.
- *
- * NOTE: The virtual inheritance solves the "diamond of death" problem.
- */
-template<class Child>
-class ParentOf : virtual public N
-{
-    private:
-        std::list<Child*> down_list_;
-    protected:
-        ParentOf() { }
-        virtual ~ParentOf() { }
-    public:
-        void addToList(Child* _down_ptr) { down_list_.push_back(_down_ptr); }
-        std::list<Child*> downList() { return down_list_; }
-        virtual void print();
-};
-
-///*
-// * Virtual inheritance solves the "diamond of death" problem.
-// */
-//template<class Sibling>
-//class SiblingOf : virtual public N
-//{
-//    private:
-//        std::list<Sibling*> side_list_;
-//    protected:
-//        SiblingOf() { }
-//        virtual ~SiblingOf() { }
-//    public:
-//        void addToList(Sibling* _side_ptr) { side_list_.push_back(_side_ptr); }
-//        std::list<Sibling*> sideList() { return side_list_; }
-//};
-
-/**
- * \brief Base class for bottom nodes.
- *
- * This class is for children that are no parents - they are bottom nodes
- *
- * It overloads the dummy 'print' function so that this is is not recursive any more.
- *
- * NOTE: The virtual inheritance solves the "diamond of death" problem.
- */
-class Bot : virtual public N
-{
-    protected:
-        virtual ~Bot() { }
-    public:
-        virtual void print();
-};
-
-//template<class Child>
-//class ExplorerOf : public ParentOf<Child>
-//{
-//    protected:
-//        ExplorerOf() { }
-//        virtual ~ExplorerOf() { }
-//    public:
-//        virtual void explore();
-//};
-
-//class Explored : public Bot
-//{
-//    protected:
-//        virtual ~Explored() { }
-//    public:
-//        virtual void explore() { }
-//};
-
-// DERIVED ISOLATED CLASSES
-
-// a bunch of fwd_decs
-class VehNode;
-class FrmNode;
-class CapNode;
-class FeaNode;
-class CorNode;
-class TrSNode;
-class SenNode;
-
-class Veh
-{
-    public:
-        Veh() : v_(1){}
-        void duplicate(){v_ *= 2;}
-        double v(){return v_;}
-    private:
-        double v_;
-};
-class Frm
-{
-    public:
-        Frm() : f_(1){}
-        void duplicate(){f_ *= 2;}
-        double f(){return f_;}
-    private:
-        double f_;
-};
-class Cap
-{
-    public:
-        Cap() : c_(1){}
-        void duplicate(){c_ *= 2;}
-        double c(){return c_;}
-    private:
-        double c_;
-};
-class Fea
-{
-    public:
-        Fea() : ft_(1){}
-        void duplicate(){ft_ *= 2;}
-        double ft(){return ft_;}
-    private:
-        double ft_;
-};
-class Cor
-{
-public:
-	Cor() : co_(1){}
-	void duplicate(){co_ *= 2;}
-	double co(){return co_;}
-private:
-	double co_;
-};
-class Sen
-{
-    public:
-        Sen() : s_(1){}
-        void duplicate(){s_ *= 2;}
-        double s(){return s_;}
-    private:
-        double s_;
-};
-
-// Derived classes for all levels of the tree
-
-class VehNode : public Veh, public ParentOf<FrmNode>, public ParentOf<SenNode>
-{
-    public:
-        VehNode() { }
-        virtual ~VehNode() { }
-        void print(); // Overload because I am Top and have both Down and Explorer children.
-};
-class FrmNode : public Frm, public ChildOf<VehNode>, public ParentOf<CapNode>
-{
-    public:
-        FrmNode(VehNode* _veh_ptr) : ChildOf<VehNode>(_veh_ptr) { }
-        virtual ~FrmNode() { }
-};
-class CapNode : public Cap, public ChildOf<FrmNode>, public ParentOf<FeaNode>//, public SiblingOf<TrSNode>
-{
-    public:
-        CapNode(FrmNode* _frm_ptr) : ChildOf<FrmNode>(_frm_ptr) { }
-        virtual ~CapNode() { }
-        void explore(); // Overload because I have both Explorer and Side children
-};
-class FeaNode : public Fea, public ChildOf<CapNode>, public ParentOf<CorNode>
-{
-    public:
-        FeaNode(CapNode* _cap_ptr) : ChildOf<CapNode>(_cap_ptr) { }
-        virtual ~FeaNode() { }
-};
-class CorNode : public Cor, public ChildOf<FeaNode>, public Bot//, public Explored
-{
-    public:
-        CorNode(FeaNode* _fea_ptr) : ChildOf<FeaNode>(_fea_ptr) { }
-        virtual ~CorNode() { }
-};
-//class TrSNode : public virtual N
-//{
-//    public:
-//        TrSNode() { }
-//        virtual ~TrSNode() { }
-//};
-class SenNode : public Sen, public ChildOf<VehNode>, public Bot
-{
-    public:
-        SenNode(VehNode* _veh_ptr) : ChildOf<VehNode>(_veh_ptr) { }
-        virtual ~SenNode() { }
-};
-
-} // namespace wolf
-
-/////////////////////
-// IMPLEMENTATIONS, here to avoid incomplete types and unwanted #includes
-/////////////////////
-
-#include <iostream>
-
-namespace wolf {
-using namespace std;
-
-template<class Child>
-void ParentOf<Child>::print()
-{
-    cout << this->id() << ":( ";
-    for (auto const & it_ptr : this->downList())
-        cout << it_ptr->id() << " ";
-    cout << ")" << endl;
-    for (auto const & it_ptr : this->downList())
-        it_ptr->print();
-}
-
-//template<class Child>
-//void ExplorerOf<Child>::explore()
-//{
-//    cout << this->id() << ":( "; // Yes I look sad but I'm OK.
-//    for (auto const & it_ptr : ParentOf<Child>::downList())
-//        cout << it_ptr->id() << " ";
-//    cout << ")" << endl;
-//    for (auto const & it_ptr : ParentOf<Child>::downList())
-//        it_ptr->explore();
-//}
-
-void Bot::print(){
-	cout << this->id() << ":( Bottom )" << endl;
-}
-
-void VehNode::print()
-{
-    cout << "Vehicle Own Field: v_ = " << v() << endl;
-    cout << "Vehicle Hardware:" << endl;
-    ParentOf < SenNode > ::print();
-    cout << "Vehicle Data:" << endl;
-    ParentOf < FrmNode > ::print();
-}
-
-//void CapNode::explore()
-//{
-//    cout << this->id() << ":( "; // Yes I look sad but I'm OK.
-//    for (auto const & it_ptr : ExplorerOf<FeaNode>::downList())
-//        cout << it_ptr->id() << " ";
-//    cout << "/ ";
-//    for (auto const & it_ptr : SiblingOf<TrSNode>::sideList())
-//        cout << it_ptr->id() << " ";
-//    cout << ")" << endl;
-//    for (auto const & it_ptr : ExplorerOf<FeaNode>::downList())
-//        it_ptr->explore();
-//}
-
-///////////////////////
-// START APPLICATION
-///////////////////////
-
-unsigned int N::id_count_ = 0;
-
-} // namespace wolf
-
-int main()
-{
-    using namespace wolf;
-
-    // Create all nodes with up-pointers already set up
-    VehNode V;
-    SenNode S0(&V), S1(&V);
-    FrmNode F0(&V), F1(&V);
-    CapNode C00(&F0), C01(&F0), C10(&F1), C11(&F1);
-    FeaNode f000(&C00), f001(&C00), f010(&C01), f011(&C01), f100(&C10), f101(&C10), f110(&C11), f111(&C11);
-//    TrSNode T0001, T0010, T0011, T0110, T0111, T1011;
-
-    // Add sensors to vehicle
-    V.ParentOf < SenNode > ::addToList(&S0);
-    V.ParentOf < SenNode > ::addToList(&S1);
-
-    // Add frames to vehicle
-    V.ParentOf < FrmNode > ::addToList(&F0);
-    V.ParentOf < FrmNode > ::addToList(&F1);
-
-    // Add captures to frames
-    F0.ParentOf<CapNode>::addToList(&C00);
-    F0.ParentOf<CapNode>::addToList(&C01);
-    F1.ParentOf<CapNode>::addToList(&C10);
-    F1.ParentOf<CapNode>::addToList(&C11);
-
-    // Add features to captures
-    C00.ParentOf<FeaNode>::addToList(&f000);
-    C00.ParentOf<FeaNode>::addToList(&f001);
-    C01.ParentOf<FeaNode>::addToList(&f010);
-    C01.ParentOf<FeaNode>::addToList(&f011);
-    C10.ParentOf<FeaNode>::addToList(&f100);
-    C10.ParentOf<FeaNode>::addToList(&f101);
-    C11.ParentOf<FeaNode>::addToList(&f110);
-    C11.ParentOf<FeaNode>::addToList(&f111);
-
-//    // Add trans-sensors to captures
-//    C00.SiblingOf<TrSNode>::addToList(&T0001);
-//    C00.SiblingOf<TrSNode>::addToList(&T0010);
-//    C00.SiblingOf<TrSNode>::addToList(&T0011);
-//    C01.SiblingOf<TrSNode>::addToList(&T0110);
-//    C01.SiblingOf<TrSNode>::addToList(&T0111);
-//    C10.SiblingOf<TrSNode>::addToList(&T1011);
-
-    // explore() : means we are calling advanced functionality from Explorer classes. Here, we just fake.
-    // print()   : means we just print linkage info.
-    cout << "V.explore():" << endl;
-//    V.explore();
-    V.duplicate();
-    cout << "V.print():" << endl;
-    V.print();
-
-    return 0;
-}
diff --git a/demos/demo_wolf_autodiffwrapper.cpp b/demos/demo_wolf_autodiffwrapper.cpp
deleted file mode 100644
index 8d0897956275c596d47782117311784b8ea2828e..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_autodiffwrapper.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
-// Testing creating wolf tree from imported .graph file
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/capture/capture_void.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-int main(int argc, char** argv) 
-{
-    using namespace wolf;
-
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-
-    if (argc != 3 || atoi(argv[2]) < 0 )
-    {
-        std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
-        std::cout << "    FILE_PATH is the .graph file path" << std::endl;
-        std::cout << "    MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    // auxiliar variables
-    std::string file_path_ = argv[1];
-    unsigned int MAX_VERTEX = atoi(argv[2]);
-    if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
-    std::ifstream offLineFile_;
-    clock_t t1;
-    ceres::Solver::Summary summary_ceres_diff, summary_wolf_diff;
-
-    // loading variables
-    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_ceres_diff;
-    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_wolf_diff;
-    std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_wolf_diff;
-
-    // Wolf problem
-    ProblemPtr wolf_problem_ceres_diff = new Problem(FRM_PO_2D);
-    ProblemPtr wolf_problem_wolf_diff = new Problem(FRM_PO_2D);
-    SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
-    // Ceres wrappers
-    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_ceres_diff = new CeresManager(wolf_problem_ceres_diff, ceres_options, false);
-    CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_wolf_diff, ceres_options, true);
-
-    // load graph from .txt
-    offLineFile_.open(file_path_.c_str(), std::ifstream::in);
-    if (offLineFile_.is_open())
-    {
-        std::string buffer;
-        unsigned int j = 0;
-        // Line by line
-        while (std::getline(offLineFile_, buffer))
-        {
-            //std::cout << "new line:" << buffer << std::endl;
-            std::string bNum;
-            unsigned int i = 0;
-
-            // VERTEX
-            if (buffer.at(0) == 'V')
-            {
-                //skip rest of VERTEX word
-                while (buffer.at(i) != ' ') i++;
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-
-                //vertex index
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int vertex_index = atoi(bNum.c_str());
-                bNum.clear();
-
-                if (vertex_index <= MAX_VERTEX+1)
-                {
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-
-                    // vertex pose
-                    Eigen::Vector3s vertex_pose;
-                    // x
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // y
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // theta
-                    while (i < buffer.size() && buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(2) = atof(bNum.c_str());
-                    bNum.clear();
-
-                    // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_ceres_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_wolf_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    wolf_problem_ceres_diff->getTrajectory()->addFrame(vertex_frame_ptr_ceres_diff);
-                    wolf_problem_wolf_diff->getTrajectory()->addFrame(vertex_frame_ptr_wolf_diff);
-                    // store
-                    index_2_frame_ptr_ceres_diff[vertex_index] = vertex_frame_ptr_ceres_diff;
-                    index_2_frame_ptr_wolf_diff[vertex_index] = vertex_frame_ptr_wolf_diff;
-                    frame_ptr_2_index_wolf_diff[vertex_frame_ptr_wolf_diff] = vertex_index;
-
-                    //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_wolf_diff->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
-                }
-            }
-            // EDGE
-            else if (buffer.at(0) == 'E')
-            {
-                j++;
-                //skip rest of EDGE word
-                while (buffer.at(i) != ' ') i++;
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-
-                //from
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int edge_old = atoi(bNum.c_str());
-                bNum.clear();
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-
-                //to index
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int edge_new = atoi(bNum.c_str());
-                bNum.clear();
-
-                if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
-                {
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-
-                    // edge vector
-                    Eigen::Vector3s edge_vector;
-                    // x
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // y
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // theta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(2) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-
-                    // edge covariance
-                    Eigen::Matrix3s edge_information;
-                    // xx
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // xy
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,1) = atof(bNum.c_str());
-                    edge_information(1,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // yy
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(1,1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // thetatheta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(2,2) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // xtheta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,2) = atof(bNum.c_str());
-                    edge_information(2,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // ytheta
-                    while (i < buffer.size() && buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(1,2) = atof(bNum.c_str());
-                    edge_information(2,1) = atof(bNum.c_str());
-                    bNum.clear();
-
-                    // add capture, feature and factor to problem
-                    FeatureBasePtr feature_ptr_ceres_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
-                    FeatureBasePtr feature_ptr_wolf_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
-                    CaptureVoid* capture_ptr_ceres_diff = new CaptureVoid(TimeStamp(0), sensor);
-                    CaptureVoid* capture_ptr_wolf_diff = new CaptureVoid(TimeStamp(0), sensor);
-                    assert(index_2_frame_ptr_ceres_diff.find(edge_old) != index_2_frame_ptr_ceres_diff.end() && "edge from vertex not added!");
-                    assert(index_2_frame_ptr_wolf_diff.find(edge_old) != index_2_frame_ptr_wolf_diff.end() && "edge from vertex not added!");
-                    FrameBasePtr frame_old_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_old];
-                    FrameBasePtr frame_old_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_old];
-                    assert(index_2_frame_ptr_ceres_diff.find(edge_new) != index_2_frame_ptr_ceres_diff.end() && "edge to vertex not added!");
-                    assert(index_2_frame_ptr_wolf_diff.find(edge_new) != index_2_frame_ptr_wolf_diff.end() && "edge to vertex not added!");
-                    FrameBasePtr frame_new_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_new];
-                    FrameBasePtr frame_new_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_new];
-                    frame_new_ptr_ceres_diff->addCapture(capture_ptr_ceres_diff);
-                    frame_new_ptr_wolf_diff->addCapture(capture_ptr_wolf_diff);
-                    capture_ptr_ceres_diff->addFeature(feature_ptr_ceres_diff);
-                    capture_ptr_wolf_diff->addFeature(feature_ptr_wolf_diff);
-                    FactorOdom2D* factor_ptr_ceres_diff = new FactorOdom2D(feature_ptr_ceres_diff, frame_old_ptr_ceres_diff);
-                    FactorOdom2D* factor_ptr_wolf_diff = new FactorOdom2D(feature_ptr_wolf_diff, frame_old_ptr_wolf_diff);
-                    feature_ptr_ceres_diff->addFactor(factor_ptr_ceres_diff);
-                    feature_ptr_wolf_diff->addFactor(factor_ptr_wolf_diff);
-                    //std::cout << "Added edge! " << factor_ptr_wolf_diff->id() << " from vertex " << factor_ptr_wolf_diff->getCapture()->getFrame()->id() << " to " << factor_ptr_wolf_diff->getFrameTo()->id() << std::endl;
-                    //std::cout << "vector " << factor_ptr_wolf_diff->getMeasurement().transpose() << std::endl;
-                    //std::cout << "information " << std::endl << edge_information << std::endl;
-                    //std::cout << "covariance " << std::endl << factor_ptr_wolf_diff->getMeasurementCovariance() << std::endl;
-                }
-            }
-            else
-                assert("unknown line");
-        }
-        printf("\nGraph loaded!\n");
-    }
-    else
-        printf("\nError opening file\n");
-
-    // PRIOR
-    FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectory()->getFrameList().front();
-    FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectory()->getFrameList().front();
-    CaptureFix* initial_covariance_ceres_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_ceres_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
-    CaptureFix* initial_covariance_wolf_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_wolf_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
-    first_frame_ceres_diff->addCapture(initial_covariance_ceres_diff);
-    first_frame_wolf_diff->addCapture(initial_covariance_wolf_diff);
-    initial_covariance_ceres_diff->process();
-    initial_covariance_wolf_diff->process();
-    //std::cout << "initial covariance: factor " << initial_covariance_wolf_diff->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_wolf_diff->getFeatureList().front()->getMeasurementCovariance() << std::endl;
-
-    // COMPUTE COVARIANCES
-    std::cout << "computing covariances..." << std::endl;
-    t1 = clock();
-    ceres_manager_ceres_diff->computeCovariances(ALL);//ALL_MARGINALS
-    double t_sigma_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC;
-    t1 = clock();
-    ceres_manager_wolf_diff->computeCovariances(ALL);//ALL_MARGINALS
-    double t_sigma_wolf = ((double) clock() - t1) / CLOCKS_PER_SEC;
-    std::cout << "computed!" << std::endl;
-
-    // SOLVING PROBLEMS
-    std::cout << "solving..." << std::endl;
-    Eigen::VectorXs prev_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
-    summary_ceres_diff = ceres_manager_ceres_diff->solve();
-    Eigen::VectorXs post_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
-    //std::cout << summary_ceres_diff.BriefReport() << std::endl;
-    Eigen::VectorXs prev_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
-    summary_wolf_diff = ceres_manager_wolf_diff->solve();
-    Eigen::VectorXs post_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
-    //std::cout << summary_wolf_diff.BriefReport() << std::endl;
-    std::cout << "solved!" << std::endl;
-
-    std::cout << "CERES AUTODIFF:" << std::endl;
-    std::cout << "Covariance computation: " << t_sigma_ceres << "s" << std::endl;
-    std::cout << "Solving:                " << summary_ceres_diff.total_time_in_seconds << "s" << std::endl;
-    std::cout << "Prev:                   " << prev_ceres_state.transpose() << std::endl;
-    std::cout << "Post:                   " << post_ceres_state.transpose() << std::endl;
-
-    std::cout << std::endl << "WOLF AUTODIFF:" << std::endl;
-    std::cout << "Covariance computation: " << t_sigma_wolf << "s" << std::endl;
-    std::cout << "Solving:                " << summary_wolf_diff.total_time_in_seconds << "s" << std::endl;
-    std::cout << "Prev:                   " << prev_wolf_state.transpose() << std::endl;
-    std::cout << "Post:                   " << post_wolf_state.transpose() << std::endl;
-
-    delete wolf_problem_ceres_diff; //not necessary to delete anything more, wolf will do it!
-    delete wolf_problem_wolf_diff; //not necessary to delete anything more, wolf will do it!
-    std::cout << "wolf_problem_ deleted!" << std::endl;
-    delete ceres_manager_ceres_diff;
-    delete ceres_manager_wolf_diff;
-    std::cout << "ceres_manager deleted!" << std::endl;
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
diff --git a/demos/demo_wolf_factories.cpp b/demos/demo_wolf_factories.cpp
deleted file mode 100644
index ce7db15c8ee5a986d8d4ed469efd9ee57dbf3351..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_factories.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/**
- * \file test_wolf_factories.cpp
- *
- *  Created on: Apr 25, 2016
- *      \author: jsola
- */
-
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_GPS_fix.h"
-#include "core/hardware/hardware_base.h"
-#include "core/sensor/sensor_camera.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "../sensor_imu.h"
-//#include "../sensor_gps.h"
-
-#include "core/processor/processor_odom_2D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "../processor_image_feature.h"
-
-#include "core/problem/problem.h"
-
-#include "core/common/factory.h"
-
-#include <iostream>
-#include <iomanip>
-#include <cstdlib>
-
-int main(void)
-{
-    using namespace wolf;
-    using namespace std;
-    using std::shared_ptr;
-    using std::make_shared;
-    using std::static_pointer_cast;
-
-    //==============================================================================================
-    std::string wolf_root       = _WOLF_ROOT_DIR;
-    std::string wolf_config     = wolf_root + "/src/examples";
-    std::cout << "\nwolf directory for configuration files: " << wolf_config << std::endl;
-    //==============================================================================================
-
-    cout << "\n====== Registering creators in the Wolf Factories =======" << endl;
-
-    cout << "If you look above, you see the registered creators.\n"
-            "There is only one attempt per class, and it is successful!\n"
-            "We do this by registering in the class\'s .cpp file.\n"
-            "\n"
-            "- See \'" << wolf_root << "/src/examples/test_wolf_factories.cpp\'\n"
-            "  for the way to install sensors and processors to wolf::Problem." << endl;
-
-    // Start creating the problem
-
-    ProblemPtr problem = Problem::create(FRM_PO_3D);
-
-    // define some useful parameters
-    Eigen::VectorXs pq_3d(7), po_2d(3), p_3d(3);
-    shared_ptr<IntrinsicsOdom2D> intr_odom2d_ptr = nullptr;
-
-    cout << "\n================== Intrinsics Factory ===================" << endl;
-
-    // Use params factory for camera intrinsics
-    IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", wolf_config + "/camera_params_ueye_sim.yaml");
-    ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE FEATURE", wolf_config + "/processor_image_feature.yaml");
-
-    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model_raw.transpose() << endl;
-//    cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl;
-
-    cout << "\n==================== Install Sensors ====================" << endl;
-
-    // Install sensors
-    problem->installSensor("CAMERA",     "front left camera",    pq_3d,  intr_cam_ptr);
-    problem->installSensor("CAMERA",     "front right camera",   pq_3d,  wolf_config + "/camera_params_ueye_sim.yaml");
-    problem->installSensor("ODOM 2D",    "main odometer",        po_2d,  intr_odom2d_ptr);
-    problem->installSensor("GPS FIX",    "GPS fix",              p_3d);
-    problem->installSensor("IMU",        "inertial",             pq_3d);
-//    problem->installSensor("GPS",        "GPS raw",              p_3d);
-    problem->installSensor("ODOM 2D",    "aux odometer",         po_2d,  intr_odom2d_ptr);
-    problem->installSensor("CAMERA", "rear camera", pq_3d, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-
-    // print available sensors
-    for (auto sen : problem->getHardware()->getSensorList())
-    {
-        cout << "Sensor " << setw(2) << left << sen->id()
-                << " | type " << setw(8) << sen->getType()
-                << " | name: " << sen->getName() << endl;
-    }
-
-    cout << "\n=================== Install Processors ===================" << endl;
-
-    // Install processors and bind them to sensors -- by sensor name!
-    problem->installProcessor("ODOM 2D", "main odometry",    "main odometer");
-    problem->installProcessor("ODOM 2D", "sec. odometry",    "aux odometer");
-    problem->installProcessor("IMU",     "pre-integrated",   "inertial");
-    problem->installProcessor("IMAGE FEATURE",   "ORB",              "front left camera", wolf_config + "/processor_image_feature.yaml");
-//    problem->createProcessor("GPS",     "GPS pseudoranges", "GPS raw");
-
-    // print installed processors
-    for (auto sen : problem->getHardware()->getSensorList())
-        for (auto prc : sen->getProcessorList())
-            cout << "Processor " << setw(2) << left  << prc->id()
-            << " | type : " << setw(8) << prc->getType()
-            << " | name: " << setw(17) << prc->getName()
-            << " | bound to sensor " << setw(2) << prc->getSensor()->id() << ": " << prc->getSensor()->getName() << endl;
-
-    return 0;
-}
-
diff --git a/demos/demo_wolf_logging.cpp b/demos/demo_wolf_logging.cpp
deleted file mode 100644
index ee3b9b5763037355fcf24ec813a79199067eb227..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_logging.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/**
- * \file test_wolf_logging.cpp
- *
- * Created on: Oct 28, 2016
- * \author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
-
-int main(int, char*[])
-{
-  WOLF_INFO("test info ", 5, " ", 0.123);
-
-  WOLF_WARN("test warn ", 5, " ", 0.123);
-
-  WOLF_ERROR("test error ", 5, " ", 0.123);
-
-  WOLF_TRACE("test trace ", 5, " ", 0.123);
-
-  WOLF_DEBUG("test debug ", 5, " ", 0.123);
-
-  return 0;
-}
diff --git a/demos/demo_wolf_prunning.cpp b/demos/demo_wolf_prunning.cpp
deleted file mode 100644
index 99567b5c3fb991e7664b255fc3893df9f027e1c8..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_prunning.cpp
+++ /dev/null
@@ -1,566 +0,0 @@
-// Testing creating wolf tree from imported .graph file
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "wolf_manager.h"
-#include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-// EIGEN
-//#include <Eigen/CholmodSupport>
-#include <Eigen/StdVector> // Eigen in std vector
-
-namespace wolf{
-// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-
-void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (int k=0; k<ins.outerSize(); ++k)
-    for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
-      original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
-
-  original.makeCompressed();
-}
-
-int main(int argc, char** argv) 
-{
-    using namespace wolf;
-
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
-
-    if (argc != 3 || atoi(argv[2]) < 0 )
-    {
-        std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
-        std::cout << "    FILE_PATH is the .graph file path" << std::endl;
-        std::cout << "    MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-
-    // auxiliar variables
-    std::string file_path_ = argv[1];
-    unsigned int MAX_VERTEX = atoi(argv[2]);
-    if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
-    std::ifstream offLineFile_;
-    clock_t t1;
-    ceres::Solver::Summary summary_full, summary_prun;
-    Eigen::MatrixXs Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
-    Eigen::MatrixXs Sigma_11(2,2), Sigma_12(2,1), Sigma_13(2,2), Sigma_14(2,1),
-                    Sigma_22(1,1), Sigma_23(1,2), Sigma_24(1,1),
-                    Sigma_33(2,2), Sigma_34(2,1),
-                    Sigma_44(1,1);
-
-    std::vector<Eigen::MatrixXs> jacobians;
-    jacobians.push_back(Eigen::MatrixXs::Zero(3,2));
-    jacobians.push_back(Eigen::MatrixXs::Zero(3,1));
-    jacobians.push_back(Eigen::MatrixXs::Zero(3,2));
-    jacobians.push_back(Eigen::MatrixXs::Zero(3,1));
-    Scalar xi, yi, thi, si, ci, xj, yj;
-    double t_sigma_manual = 0;
-
-    // loading variables
-    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full;
-    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_prun;
-    std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
-
-    // Wolf problem
-    ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
-    ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
-    SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
-    Eigen::SparseMatrix<Scalar> Lambda(0,0);
-
-    // prunning
-    FactorBasePtrList ordered_fac_ptr;
-    std::list<Scalar> ordered_ig;
-
-    // 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_full = new CeresManager(wolf_problem_full, ceres_options);
-    CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun,ceres_options);
-
-    // load graph from .txt
-    offLineFile_.open(file_path_.c_str(), std::ifstream::in);
-    if (offLineFile_.is_open())
-    {
-        std::string buffer;
-        unsigned int j = 0;
-        // Line by line
-        while (std::getline(offLineFile_, buffer))
-        {
-            //std::cout << "new line:" << buffer << std::endl;
-            std::string bNum;
-            unsigned int i = 0;
-
-            // VERTEX
-            if (buffer.at(0) == 'V')
-            {
-                //skip rest of VERTEX word
-                while (buffer.at(i) != ' ') i++;
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-
-                //vertex index
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int vertex_index = atoi(bNum.c_str());
-                bNum.clear();
-
-                if (vertex_index <= MAX_VERTEX+1)
-                {
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-
-                    // vertex pose
-                    Eigen::Vector3s vertex_pose;
-                    // x
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // y
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // theta
-                    while (i < buffer.size() && buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    vertex_pose(2) = atof(bNum.c_str());
-                    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)));
-                    wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
-                    wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
-                    // store
-                    index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
-                    index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
-                    frame_ptr_2_index_prun[vertex_frame_ptr_prun] = vertex_index;
-                    // Information matrix
-                    Lambda.conservativeResize(Lambda.rows()+3,Lambda.cols()+3);
-
-                    //std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr_prun->id() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
-                }
-            }
-            // EDGE
-            else if (buffer.at(0) == 'E')
-            {
-                j++;
-                //skip rest of EDGE word
-                while (buffer.at(i) != ' ') i++;
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-
-                //from
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int edge_old = atoi(bNum.c_str());
-                bNum.clear();
-                //skip white spaces
-                while (buffer.at(i) == ' ') i++;
-
-                //to index
-                while (buffer.at(i) != ' ')
-                    bNum.push_back(buffer.at(i++));
-                unsigned int edge_new = atoi(bNum.c_str());
-                bNum.clear();
-
-                if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
-                {
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-
-                    // edge vector
-                    Eigen::Vector3s edge_vector;
-                    // x
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // y
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // theta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_vector(2) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-
-                    // edge covariance
-                    Eigen::Matrix3s edge_information;
-                    // xx
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // xy
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,1) = atof(bNum.c_str());
-                    edge_information(1,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // yy
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(1,1) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // thetatheta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(2,2) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // xtheta
-                    while (buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(0,2) = atof(bNum.c_str());
-                    edge_information(2,0) = atof(bNum.c_str());
-                    bNum.clear();
-                    //skip white spaces
-                    while (buffer.at(i) == ' ') i++;
-                    // ytheta
-                    while (i < buffer.size() && buffer.at(i) != ' ')
-                        bNum.push_back(buffer.at(i++));
-                    edge_information(1,2) = atof(bNum.c_str());
-                    edge_information(2,1) = atof(bNum.c_str());
-                    bNum.clear();
-
-                    //std::cout << "Adding edge... " << std::endl;
-                    // add capture, feature and factor to problem
-                    FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
-                    FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
-                    CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
-                    CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
-                    assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
-                    assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && "edge from vertex not added!");
-                    FrameBasePtr frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
-                    FrameBasePtr frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
-                    assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && "edge to vertex not added!");
-                    assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && "edge to vertex not added!");
-                    FrameBasePtr frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
-                    FrameBasePtr frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
-                    frame_new_ptr_full->addCapture(capture_ptr_full);
-                    frame_new_ptr_prun->addCapture(capture_ptr_prun);
-                    capture_ptr_full->addFeature(feature_ptr_full);
-                    capture_ptr_prun->addFeature(feature_ptr_prun);
-                    FactorOdom2DAnalytic* factor_ptr_full = new FactorOdom2DAnalytic(feature_ptr_full, frame_old_ptr_full);
-                    FactorOdom2DAnalytic* factor_ptr_prun = new FactorOdom2DAnalytic(feature_ptr_prun, frame_old_ptr_prun);
-                    feature_ptr_full->addFactor(factor_ptr_full);
-                    feature_ptr_prun->addFactor(factor_ptr_prun);
-                    //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameOther()->id() << std::endl;
-                    //std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
-                    //std::cout << "information " << std::endl << edge_information << std::endl;
-                    //std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
-
-                    t1 = clock();
-                    Scalar xi = *(frame_old_ptr_prun->getP()->get());
-                    Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
-                    Scalar thi = *(frame_old_ptr_prun->getO()->get());
-                    Scalar si = sin(thi);
-                    Scalar ci = cos(thi);
-                    Scalar xj = *(frame_new_ptr_prun->getP()->get());
-                    Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
-                    Eigen::MatrixXs Ji(3,3), Jj(3,3);
-                    Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
-                           si,-ci,-(xj-xi)*ci-(yj-yi)*si,
-                            0,  0,                    -1;
-                    Jj <<  ci, si, 0,
-                          -si, ci, 0,
-                            0,  0, 1;
-                    //std::cout << "Ji" << std::endl << Ji << std::endl;
-                    //std::cout << "Jj" << std::endl << Jj << std::endl;
-                    Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
-                    insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3);
-                    insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3);
-                    insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3);
-                    insertSparseBlock((Jj.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_new*3, edge_old*3);
-                    Lambda = Lambda + DeltaLambda;
-                    t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
-                }
-            }
-            else
-                assert("unknown line");
-        }
-        printf("\nGraph loaded!\n");
-    }
-    else
-        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();
-    CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
-    CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
-    first_frame_full->addCapture(initial_covariance_full);
-    first_frame_prun->addCapture(initial_covariance_prun);
-    initial_covariance_full->process();
-    initial_covariance_prun->process();
-    //std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
-    t1 = clock();
-    Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
-    insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
-    Lambda = Lambda + DeltaLambda;
-    t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-    // COMPUTE COVARIANCES
-    FactorBasePtrList factors;
-    wolf_problem_prun->getTrajectory()->getFactorList(factors);
-    // Manual covariance computation
-    t1 = clock();
-    Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda);  // performs a Cholesky factorization of A
-    Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols()));
-    t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
-    //std::cout << "Lambda" << std::endl << Lambda << std::endl;
-    //std::cout << "Sigma" << std::endl << Sigma << std::endl;
-
-    std::cout << " ceres is computing covariances..." << std::endl;
-    t1 = clock();
-    ceres_manager_prun->computeCovariances(ALL);//ALL_MARGINALS
-    double t_sigma_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC;
-    std::cout << "computed!" << std::endl;
-
-    t1 = clock();
-
-    for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
-    {
-        if ((*c_it)->getCategory() != FAC_FRAME) continue;
-
-        // Measurement covariance
-        Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
-        //std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
-        //std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
-
-        // NEW WAY
-        // State covariance
-        //11
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_11);
-        //12
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_12);
-        //13
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_13);
-        //14
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_14);
-
-        //22
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_22);
-        //23
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_23);
-        //24
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_24);
-
-        //33
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_33);
-        //34
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_34);
-
-        //44
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_44);
-
-//        std::cout << "Sigma_11" << std::endl << Sigma_11 << std::endl;
-//        std::cout << "Sigma_12" << std::endl << Sigma_12 << std::endl;
-//        std::cout << "Sigma_13" << std::endl << Sigma_13 << std::endl;
-//        std::cout << "Sigma_14" << std::endl << Sigma_14 << std::endl;
-//        std::cout << "Sigma_22" << std::endl << Sigma_22 << std::endl;
-//        std::cout << "Sigma_23" << std::endl << Sigma_23 << std::endl;
-//        std::cout << "Sigma_24" << std::endl << Sigma_24 << std::endl;
-//        std::cout << "Sigma_33" << std::endl << Sigma_33 << std::endl;
-//        std::cout << "Sigma_34" << std::endl << Sigma_34 << std::endl;
-//        std::cout << "Sigma_44" << std::endl << Sigma_44 << std::endl;
-
-        // jacobians
-        ((FactorAnalytic*)(*c_it))->evaluatePureJacobians(jacobians);
-        Eigen::MatrixXs& J1 = jacobians[0];
-        Eigen::MatrixXs& J2 = jacobians[1];
-        Eigen::MatrixXs& J3 = jacobians[2];
-        Eigen::MatrixXs& J4 = jacobians[3];
-//        std::cout << "J1" << std::endl << J1 << std::endl;
-//        std::cout << "J2" << std::endl << J2 << std::endl;
-//        std::cout << "J3" << std::endl << J3 << std::endl;
-//        std::cout << "J4" << std::endl << J4 << std::endl;
-
-        // Information gain
-        Scalar IG_new = 0.5 * log( Sigma_z.determinant() /
-                                 ( Sigma_z - (J1 * Sigma_11 * J1.transpose() +
-                                              J1 * Sigma_12 * J2.transpose() +
-                                              J1 * Sigma_13 * J3.transpose() +
-                                              J1 * Sigma_14 * J4.transpose() +
-                                              J2 * Sigma_12.transpose() * J1.transpose() +
-                                              J2 * Sigma_22 * J2.transpose() +
-                                              J2 * Sigma_23 * J3.transpose() +
-                                              J2 * Sigma_24 * J4.transpose() +
-                                              J3 * Sigma_13.transpose() * J1.transpose() +
-                                              J3 * Sigma_23.transpose() * J2.transpose() +
-                                              J3 * Sigma_33 * J3.transpose() +
-                                              J3 * Sigma_34 * J4.transpose() +
-                                              J4 * Sigma_14.transpose() * J1.transpose() +
-                                              J4 * Sigma_24.transpose() * J2.transpose() +
-                                              J4 * Sigma_34.transpose() * J3.transpose() +
-                                              J4 * Sigma_44 * J4.transpose()) ).determinant() );
-
-//        std::cout << "part = " << std::endl << (J1 * Sigma_11 * J1.transpose() +
-//                                                  J1 * Sigma_12 * J2.transpose() +
-//                                                  J1 * Sigma_13 * J3.transpose() +
-//                                                  J1 * Sigma_14 * J4.transpose() +
-//                                                  J2 * Sigma_12.transpose() * J1.transpose() +
-//                                                  J2 * Sigma_22 * J2.transpose() +
-//                                                  J2 * Sigma_23 * J3.transpose() +
-//                                                  J2 * Sigma_24 * J4.transpose() +
-//                                                  J3 * Sigma_13.transpose() * J1.transpose() +
-//                                                  J3 * Sigma_23.transpose() * J2.transpose() +
-//                                                  J3 * Sigma_33 * J3.transpose() +
-//                                                  J3 * Sigma_34 * J4.transpose() +
-//                                                  J4 * Sigma_14.transpose() * J1.transpose() +
-//                                                  J4 * Sigma_24.transpose() * J2.transpose() +
-//                                                  J4 * Sigma_34.transpose() * J3.transpose() +
-//                                                  J4 * Sigma_44 * J4.transpose()) << std::endl;
-        std::cout << "IG_new = " << IG_new << std::endl;
-
-        // OLD WAY
-        // ii (old)
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
-//        std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-//        std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
-        // jj (new)
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
-//        std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-//        std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
-        // ij (old-new)
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
-        wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
-//        std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-//        std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
-
-        //jacobian
-        xi = *(*c_it)->getFrameOther()->getP()->get();
-        yi = *((*c_it)->getFrameOther()->getP()->get()+1);
-        thi = *(*c_it)->getFrameOther()->getO()->get();
-        si = sin(thi);
-        ci = cos(thi);
-        xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
-        yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
-
-        Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
-               si,-ci,-(xj-xi)*ci-(yj-yi)*si,
-                0,  0,                    -1;
-        Jj <<  ci, si, 0,
-              -si, ci, 0,
-                0,  0, 1;
-//        std::cout << "Ji" << std::endl << Ji << std::endl;
-//        std::cout << "Jj" << std::endl << Jj << std::endl;
-
-        //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
-        // Information gain
-        Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
-
-//        std::cout << "part = " << std::endl << (Ji * Sigma_ii * Ji.transpose() +
-//                                                Jj * Sigma_jj * Jj.transpose() +
-//                                                Ji * Sigma_ij * Jj.transpose() +
-//                                                Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
-        std::cout << "IG = " << IG << std::endl;
-
-        std::cout << "difference IG = " << std::abs(IG - IG_new) << std::endl;
-        assert((std::abs((IG - IG_new)/IG) < 0.1 || std::isnan(IG - IG_new)) && "not equals information gains");
-
-        if (IG < 2 && IG > 0 && !std::isnan(IG))
-        {
-            // Store as a candidate to be prunned, ordered by information gain
-            auto ordered_fac_it = ordered_fac_ptr.begin();
-            for (auto ordered_ig_it = ordered_ig.begin(); ordered_ig_it != ordered_ig.end(); ordered_ig_it++, ordered_fac_it++ )
-                if (IG < (*ordered_ig_it))
-                {
-                    ordered_ig.insert(ordered_ig_it, IG);
-                    ordered_fac_ptr.insert(ordered_fac_it, (*c_it));
-                    break;
-                }
-            ordered_ig.insert(ordered_ig.end(), IG);
-            ordered_fac_ptr.insert(ordered_fac_ptr.end(), (*c_it));
-        }
-    }
-
-    // PRUNNING
-    std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectory()->getFrameList().size(), false);
-    for (auto c_it = ordered_fac_ptr.begin(); c_it != ordered_fac_ptr.end(); c_it++ )
-    {
-        // Check heuristic: factor do not share node with any inactive factor
-        unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()];
-        unsigned int& index_frame_other = frame_ptr_2_index_prun[(*c_it)->getFrameOther()];
-
-        if (!any_inactive_in_frame[index_frame] && !any_inactive_in_frame[index_frame_other])
-        {
-            std::cout << "setting inactive" << (*c_it)->id() << std::endl;
-            (*c_it)->setStatus(FAC_INACTIVE);
-            std::cout << "set!" << std::endl;
-            any_inactive_in_frame[index_frame] = true;
-            any_inactive_in_frame[index_frame_other] = true;
-        }
-    }
-
-    double t_ig = ((double) clock() - t1) / CLOCKS_PER_SEC;
-    std::cout << "manual sigma computation " << t_sigma_manual << "s" << std::endl;
-    std::cout << "ceres sigma computation " << t_sigma_ceres << "s" << std::endl;
-    std::cout << "information gain computation " << t_ig << "s" << std::endl;
-
-    // SOLVING PROBLEMS
-    std::cout << "FULL PROBLEM" << std::endl;
-    std::cout << "solving..." << std::endl;
-    summary_full = ceres_manager_full->solve();
-    std::cout << summary_full.FullReport() << std::endl;
-    std::cout << "PRUNNED PROBLEM" << std::endl;
-    std::cout << "solving..." << std::endl;
-    summary_prun = ceres_manager_prun->solve();
-    std::cout << summary_prun.FullReport() << std::endl;
-
-    delete wolf_problem_full; //not necessary to delete anything more, wolf will do it!
-    std::cout << "wolf_problem_ deleted!" << std::endl;
-    delete ceres_manager_full;
-    delete ceres_manager_prun;
-    std::cout << "ceres_manager deleted!" << std::endl;
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
diff --git a/demos/demo_wolf_root.cpp b/demos/demo_wolf_root.cpp
deleted file mode 100644
index 4ea048471753a28281c01dc50f3fcda0316f0bd7..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_root.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-/**
- * \file test_wolf_root.cpp
- *
- * Created on: Apr 12, 2016
- * \author: Jeremie Deray
- */
-
-//Wolf
-#include "core/common/wolf.h"
-
-//std
-#include <iostream>
-
-int main(int /*argc*/, char** /*argv*/)
-{
-  std::cout << "Your wolf root directory is (_WOLF_ROOT_DIR) : " << _WOLF_ROOT_DIR << std::endl;
-
-  return 1;
-}
diff --git a/demos/demo_wolf_tree.cpp b/demos/demo_wolf_tree.cpp
deleted file mode 100644
index 9a0075a93c089db05f9bb044ec9c5f90f237228f..0000000000000000000000000000000000000000
--- a/demos/demo_wolf_tree.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-// Testing create and delete full wolf tree with odometry captures
-
-//std includes
-#include <iostream>
-#include <memory>
-#include <random>
-#include <cmath>
-#include <queue>
-
-//Wolf includes
-#include "wolf_manager.h"
-
-int main(int argc, char** argv) 
-{
-    using namespace wolf;
-
-    //Welcome message
-    std::cout << std::endl << " ========= WOLF TREE test ===========" << std::endl << std::endl;
-
-    SensorOdom2D* odom_sensor_ptr_ = new SensorOdom2D(std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
-                                                      std::make_shared<StateBlock>(Eigen::Vector1s::Zero()), 0.1, 0.1);
-    //std::cout << " odom sensor created!" << std::endl;
-
-    WolfManager* wolf_manager_ = new WolfManager(FRM_PO_2D,                             //frame structure
-                                                 odom_sensor_ptr_,                  //odom sensor
-                                                 Eigen::Vector3s::Zero(),           //prior
-                                                 Eigen::Matrix3s::Identity()*0.01,  //prior cov
-                                                 5,                                 //window size
-                                                 1);                                //time for new keyframe
-    //std::cout << " wolf_manager_ created!" << std::endl;
-
-    wolf_manager_->addSensor(odom_sensor_ptr_);
-    //std::cout << " odom sensor added!" << std::endl;
-
-    //main loop
-    for (unsigned int ii = 0; ii<1000; ii++)
-    {
-        // Add sintetic odometry
-        wolf_manager_->addCapture(new CaptureOdom2D(TimeStamp(ii*0.1),
-                                                    TimeStamp(ii*0.1+0.01),
-                                                    odom_sensor_ptr_,
-                                                    Eigen::Vector3s(0.1, 0. ,0.05)));
-        //std::cout << " capture added!" << std::endl;
-
-        // update wolf tree
-        wolf_manager_->update();
-        //std::cout << " updated!" << std::endl;
-    }
-    //std::cout << " end for!" << std::endl;
-
-    delete wolf_manager_; //not necessary to delete anything more, wolf will do it!
-
-    //End message
-    std::cout << " =========================== END ===============================" << std::endl << std::endl;
-       
-    //exit
-    return 0;
-}
-
diff --git a/demos/demo_yaml.cpp b/demos/demo_yaml.cpp
deleted file mode 100644
index 5768f4b50ce0963ba4974baa10d27c4ffc1e2382..0000000000000000000000000000000000000000
--- a/demos/demo_yaml.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/**
- * \file yaml-test.cpp
- *
- *  Created on: May 1, 2016
- *      \author: jsola
- */
-
-#include "core/math/pinhole_tools.h"
-#include "yaml/yaml_conversion.h"
-#include "processor_image_feature.h"
-#include "core/common/factory.h"
-
-#include <yaml-cpp/yaml.h>
-
-#include <eigen3/Eigen/Dense>
-
-#include <iostream>
-#include <fstream>
-
-int main()
-{
-
-    //=============================================================================================
-    std::string wolf_root       = _WOLF_ROOT_DIR;
-    std::cout << "\nwolf root directory: " << wolf_root << std::endl;
-    //=============================================================================================
-
-    using namespace Eigen;
-    using namespace wolf;
-    using std::string;
-    using YAML::Node;
-
-    // Camera parameters
-
-    YAML::Node camera_config = YAML::LoadFile(wolf_root + "/src/examples/camera.yaml");
-
-    if (camera_config["sensor type"])
-    {
-        std::string sensor_type = camera_config["sensor type"].as<std::string>();
-
-        std::string sensor_name = camera_config["sensor name"].as<std::string>();
-
-        YAML::Node params   = camera_config["intrinsic"];
-
-        // convert yaml to Eigen
-        Vector3s pos        = camera_config["extrinsic"]["position"].as<Vector3s>();
-        Vector3s ori        = camera_config["extrinsic"]["orientation"].as<Vector3s>() * M_PI / 180;
-        Vector2s size       = params["image size"].as<Vector2s>();
-        Vector4s intrinsic  = params["pinhole model"].as<Vector4s>();
-        VectorXs distortion = params["distortion"].as<VectorXs>();
-
-        // compute correction model
-        VectorXs correction(distortion.size());
-        pinhole::computeCorrectionModel(intrinsic, distortion, correction);
-
-        // output
-        std::cout << "sensor type       : " << sensor_type << std::endl;
-        std::cout << "sensor name       : " << sensor_name << std::endl;
-        std::cout << "sensor extrinsics : " << std::endl;
-        std::cout << "\tposition        : " << pos.transpose() << std::endl;
-        std::cout << "\torientation     : " << ori.transpose() << std::endl;
-        std::cout << "sensor parameters : " << std::endl;
-        std::cout << "\timage size      : " << size.transpose() << std::endl;
-        std::cout << "\tpinhole model   : " << intrinsic.transpose() << std::endl;
-        std::cout << "\tdistoriton      : " << distortion.transpose() << std::endl;
-        std::cout << "\tcorrection      : " << correction.transpose() << std::endl;
-    }
-    else
-        std::cout << "Bad configuration file. No sensor type found." << std::endl;
-
-//    // Processor Image parameters
-//
-//    ProcessorParamsImage p;
-//
-//    Node params = YAML::LoadFile(wolf_root + "/src/examples/processor_image_feature.yaml");
-//
-//    if (params["processor type"])
-//    {
-//        Node as = params["active search"];
-//        p.active_search.grid_width      = as["grid width"].as<unsigned int>();
-//        p.active_search.grid_height     = as["grid height"].as<unsigned int>();
-//        p.active_search.separation      = as["separation"].as<unsigned int>();
-//
-//        Node img = params["image"];
-//        p.image.width                   = img["width"].as<unsigned int>();
-//        p.image.height                  = img["height"].as<unsigned int>();
-//
-//        Node alg = params["algorithm"];
-//        p.max_new_features            = alg["maximum new features"].as<unsigned int>();
-//        p.min_features_for_keyframe   = alg["minimum features for new keyframe"].as<unsigned int>();
-//    }
-
-    return 0;
-}
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index b75801bde13cff3a003e0ba746bec5fb864bb813..cd7ba019179d7bfb7aa61723073028ccc3b17024 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -25,15 +25,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 
     // 1. get KF
     FrameBasePtr kf(nullptr);
-    if ( !kf_pack_buffer_.empty() )
+    if ( !buffer_pack_kf_.empty() )
     {
         // KeyFrame Callback received
-        PackKeyFramePtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
+        PackKeyFramePtr pack = buffer_pack_kf_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
 
         if (pack!=nullptr)
             kf = pack->key_frame;
 
-        kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() );
+        buffer_pack_kf_.removeUpTo( _capture->getTimeStamp() );
 
         assert( kf && "Callback KF is not close enough to _capture!");
     }
diff --git a/include/core/factor/factor_feature_dummy.h b/include/core/factor/factor_feature_dummy.h
index 4731ecaac631c57db2be44315b1d32084e03fd13..83e07bb49060279665ba9002798989be5693440a 100644
--- a/include/core/factor/factor_feature_dummy.h
+++ b/include/core/factor/factor_feature_dummy.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_EPIPOLAR_H
-#define FACTOR_EPIPOLAR_H
+#ifndef FACTOR_FEATURE_DUMMY_H
+#define FACTOR_FEATURE_DUMMY_H
 
 #include "core/factor/factor_base.h"
 
@@ -52,7 +52,7 @@ class FactorFeatureDummy : public FactorBase
 inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
                                               const ProcessorBasePtr& _processor_ptr,
                                               bool _apply_loss_function, FactorStatus _status) :
-        FactorBase("EPIPOLAR", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+        FactorBase("FEATURE DUMMY", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
 {
     //
 }
@@ -65,4 +65,4 @@ inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_p
 
 } // namespace wolf
 
-#endif // FACTOR_EPIPOLAR_H
+#endif // FACTOR_FEATURE_DUMMY_H
diff --git a/include/core/factor/factor_landmark_dummy.h b/include/core/factor/factor_landmark_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..95a5c483dc4dfd336880b64972f4ecad4fcb07bf
--- /dev/null
+++ b/include/core/factor/factor_landmark_dummy.h
@@ -0,0 +1,68 @@
+#ifndef FACTOR_LANDMARK_DUMMY_H
+#define FACTOR_LANDMARK_DUMMY_H
+
+#include "core/factor/factor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorLandmarkDummy);
+
+class FactorLandmarkDummy : public FactorBase
+{
+    public:
+        FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr,
+                            const LandmarkBasePtr& _landmark_other_ptr,
+                            const ProcessorBasePtr& _processor_ptr = nullptr,
+                            bool _apply_loss_function = false,
+                            FactorStatus _status = FAC_ACTIVE);
+
+        virtual ~FactorLandmarkDummy() = default;
+
+        /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
+        **/
+        virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};
+
+        /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+         **/
+        virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override {};
+
+        /** \brief Returns the jacobians computation method
+         **/
+        virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
+
+        /** \brief Returns a vector of pointers to the states in which this factor depends
+         **/
+        virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+
+        /** \brief Returns the factor residual size
+         **/
+        virtual unsigned int getSize() const override {return 0;}
+
+        /** \brief Returns the factor states sizes
+         **/
+        virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
+
+    public:
+        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
+                                              const NodeBasePtr& _correspondant_ptr,
+                                              const ProcessorBasePtr& _processor_ptr = nullptr);
+
+};
+
+inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& /*_feature_ptr*/, const LandmarkBasePtr& _landmark_other_ptr,
+                                                const ProcessorBasePtr& _processor_ptr,
+                                                bool _apply_loss_function, FactorStatus _status) :
+        FactorBase("FEATURE DUMMY", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status)
+{
+    //
+}
+
+inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+                                                 const ProcessorBasePtr& _processor_ptr)
+{
+    return std::make_shared<FactorLandmarkDummy>(_feature_ptr, std::static_pointer_cast<LandmarkBase>(_correspondant_ptr), _processor_ptr);
+}
+
+} // namespace wolf
+
+#endif // FACTOR_LANDMARK_DUMMY_H
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 44e1cfbd2981f6d7afe67311c249b9fc67897638..0fdb349c15db78a40fbe2b658c739c776a9b2c3d 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -80,6 +80,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         bool isKeyOrAux() const;
 
         // set type
+        void setNonEstimated();
         void setKey();
         void setAux();
 
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 839ee0f97c9694da18cff81b17e78ce853b7e934..373b20149b9202ca61fcfb1c1ea4d265e284f63f 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -44,8 +44,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
          * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
          *
          **/
-    LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
-    LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+        LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
         virtual ~LandmarkBase();
         virtual void remove();
         virtual YAML::Node saveToYaml() const;
@@ -96,6 +95,11 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         template<typename classType, typename... T>
         static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
 
+        /** \brief Creator for Factory<LandmarkBase, YAML::Node>
+         * Caution: This creator does not set the landmark's anchor frame and sensor.
+         * These need to be set afterwards.
+         */
+        static LandmarkBasePtr create(const YAML::Node& _node);
 };
 
 }
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 6b2e32db108077716f372deb7fc4b2fc4438a6c6..fdfb8a1052e4dabda74f7060c4e6c0128d6c29cd 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -61,7 +61,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     public:
         static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
-        static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file);
+        static ProblemPtr autoSetup(const std::string& _yaml_file);
         virtual ~Problem();
 
         // Properties -----------------------------------------
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 93f70607ade2edb45cb9b0c11f0a392657ab4241..b88d6714f0c7ea715b64f4799fde91dedefd5224 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -115,7 +115,7 @@ protected:
  *
  * Object and functions to manage a buffer of KFPack objects.
  */
-class PackKeyFrameBuffer : public Buffer<PackKeyFramePtr>
+class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
 {
     public:
 
@@ -196,12 +196,11 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
     protected:
         unsigned int processor_id_;
         ProcessorParamsBasePtr params_;
-        PackKeyFrameBuffer kf_pack_buffer_;
+        BufferPackKeyFrame buffer_pack_kf_;
         BufferCapture buffer_capture_;
 
     private:
         SensorBaseWPtr sensor_ptr_;
-
         static unsigned int processor_id_count_;
 
     public:
diff --git a/include/core/processor/processor_tracker_feature_dummy.h b/include/core/processor/processor_tracker_feature_dummy.h
index f16aa35f9ab1475ba5c2cc807de3fbb81dffc683..150a6ffb0068b723c9f237918b81dc56116323e2 100644
--- a/include/core/processor/processor_tracker_feature_dummy.h
+++ b/include/core/processor/processor_tracker_feature_dummy.h
@@ -15,6 +15,21 @@
 namespace wolf
 {
     
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureDummy);
+
+struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
+{
+    unsigned int n_tracks_lost; ///< number of tracks lost each time track is called (the first ones)
+
+    ProcessorParamsTrackerFeatureDummy() = default;
+    ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::paramsServer & _server):
+        ProcessorParamsTrackerFeature(_unique_name, _server)
+    {
+        n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost", "1");
+    }
+};
+
+
 WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
     
 //Class
@@ -22,14 +37,13 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
 {
 
     public:
-        ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
+        ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature);
         virtual ~ProcessorTrackerFeatureDummy();
         virtual void configure(SensorBasePtr _sensor) { };
 
     protected:
 
-        static unsigned int count_;
-        unsigned int n_feature_;
+        ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_;
 
         /** \brief Track provided features from \b last to \b incoming
          * \param _features_last_in input list of features in \b last to track
@@ -69,11 +83,17 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
+    public:
+
+        static ProcessorBasePtr create(const std::string& _unique_name,
+                                       const ProcessorParamsBasePtr _params,
+                                       const SensorBasePtr sensor_ptr = nullptr);
+
 };
 
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
-        ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature),
-        n_feature_(0)
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
+        ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature_dummy),
+        params_tracker_feature_dummy_(_params_tracker_feature_dummy)
 {
     //
 }
diff --git a/include/core/processor/processor_tracker_landmark_dummy.h b/include/core/processor/processor_tracker_landmark_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..804ecc9869789b9326d0b572c210884c8f9d15b3
--- /dev/null
+++ b/include/core/processor/processor_tracker_landmark_dummy.h
@@ -0,0 +1,92 @@
+/**
+ * \file processor_tracker_landmark_dummy.h
+ *
+ *  Created on: Apr 12, 2016
+ *      \author: jvallve
+ */
+
+#ifndef PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
+#define PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
+
+#include "core/processor/processor_tracker_landmark.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkDummy);
+
+struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandmark
+{
+    unsigned int n_landmarks_lost; ///< number of landmarks lost each time findLandmarks is called (the last ones)
+
+    ProcessorParamsTrackerLandmarkDummy() = default;
+    ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::paramsServer & _server):
+        ProcessorParamsTrackerLandmark(_unique_name, _server)
+    {
+        n_landmarks_lost = _server.getParam<unsigned int>(_unique_name + "/n_landmarks_lost", "1");
+    }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
+
+class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
+{
+    public:
+        ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
+        virtual ~ProcessorTrackerLandmarkDummy();
+        virtual void configure(SensorBasePtr _sensor) { };
+
+    protected:
+
+        ProcessorParamsTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
+
+        //virtual void preProcess() { }
+        //virtual void postProcess();
+
+        /** \brief Find provided landmarks in the incoming capture
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
+         * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+         */
+        virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
+                                           LandmarkMatchMap& _feature_landmark_correspondences);
+
+        /** \brief Vote for KeyFrame generation
+         *
+         * If a KeyFrame criterion is validated, this function returns true,
+         * meaning that it wants to create a KeyFrame at the \b last Capture.
+         *
+         * WARNING! This function only votes! It does not create KeyFrames!
+         */
+        virtual bool voteForKeyFrame();
+
+        /** \brief Detect new Features
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
+         * \return The number of detected Features.
+         *
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
+         *
+         * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
+         */
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
+
+        /** \brief Create one landmark
+         *
+         * Implement in derived classes to build the type of landmark you need for this tracker.
+         */
+        virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+
+        /** \brief Create a new factor
+         * \param _feature_ptr pointer to the Feature to constrain
+         * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
+         *
+         * Implement this method in derived classes.
+         */
+        virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+};
+
+} // namespace wolf
+
+#endif /* PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ */
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 511f22a096d830d898049fa966ad1d63ab42c624..edc6f9092f96fe7835d80658b975eb127552a00c 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -63,12 +63,12 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> >  TrackMatches; // mat
  *
  * these fields of FeatureBase are initialized each time a feature is added to the track matrix:
  *
- *      add(Cap, track_id, f) will set f.capture_ptr = C and f.traci_id = traci_id.
+ *      add(Cap, track_id, f) will set f.capture_ptr = C and f.track_id = track_id.
  *
  * so e.g. given a feature f,
  *
  *      getTrack   (f->trackId()) ;       // returns all the track where feature f is.
- *      getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f.
+ *      getSnapshot(f->getCapture()) ;    // returns all the features in the same capture of f.
  *
  */
 
@@ -78,13 +78,14 @@ class TrackMatrix
         TrackMatrix();
         virtual ~TrackMatrix();
 
+        // tracks across all Captures
         void            newTrack    (CaptureBasePtr _cap, FeatureBasePtr _ftr);
         void            add         (size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr);
         void            remove      (FeatureBasePtr _ftr);
         void            remove      (size_t _track_id);
         void            remove      (CaptureBasePtr _cap);
-        SizeStd          numTracks   ();
-        SizeStd          trackSize   (size_t _track_id);
+        SizeStd         numTracks   ();
+        SizeStd         trackSize   (size_t _track_id);
         Track           track       (size_t _track_id);
         Snapshot        snapshot    (CaptureBasePtr _capture);
         vector<FeatureBasePtr>
@@ -97,15 +98,24 @@ class TrackMatrix
         FeatureBasePtr  feature     (size_t _track_id, CaptureBasePtr _cap);
         CaptureBasePtr  firstCapture(size_t _track_id);
 
+        // tracks across captures that belong to keyframe
+//        SizeStd         numKeyframeTracks();
+        Track           trackAtKeyframes(size_t _track_id);
+//        bool            markKeyframe(CaptureBasePtr _capture);
+//        bool            unmarkKeyframe(CaptureBasePtr _capture);
+
     private:
 
         static SizeStd track_id_count_;
 
         // Along track: maps of Feature pointers indexed by time stamp.
+        // tracks across all Captures
         map<size_t, Track > tracks_;       // map indexed by track_Id   of ( maps indexed by TimeStamp  of ( features ) )
+//        // tracks across captures that belong to keyframe
+//        map<size_t, Track > tracks_kf_;    // map indexed by track_Id   of ( maps indexed by TimeStamp  of ( features ) )
 
         // Across track: maps of Feature pointers indexed by track_Id.
-        map<size_t, Snapshot > snapshots_; // map indexed by capture_Id of ( maps indexed by track_Id of ( features ) )
+        map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
 };
 
 } /* namespace wolf */
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 78ff43caa595ff4f8db37b0b860b3aef4ebf66fd..4a1903a4c39f83d3ce3d686127fb6218a29b4e3e 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -66,6 +66,8 @@ class parserYAML {
     bool _relative_path;
     string _path_root;
     vector<array<string, 3>> _callbacks;
+    YAML::Node problem;
+    std::string generatePath(std::string);
 public:
     parserYAML(){
         _params = map<string, string>();
@@ -96,7 +98,10 @@ public:
         _paramsProc = vector<ParamsInitProcessor>();
         _files = vector<string>();
         _file = file;
-        _path_root = path_root;
+
+        regex r("/$");
+        if(not regex_match(path_root, r)) _path_root = path_root + "/";
+        else _path_root = path_root;
         _relative_path = true;
         _callbacks = vector<array<string, 3>>();
     }
@@ -114,10 +119,21 @@ public:
     vector<array<string, 3>> processorsSerialization();
     vector<string> getFiles();
     vector<array<string, 3>> getCallbacks();
+    vector<array<string, 2>> getProblem();
     map<string,string> getParams();
     void parse();
     map<string, string> fetchAsMap(YAML::Node);
 };
+std::string parserYAML::generatePath(std::string path){
+    regex r("^/.*");
+    if(regex_match(path, r)){
+        std::cout << "Generating path " << path << std::endl;
+        return path;
+    }else{
+        std::cout << "Generating path " << _path_root + path << std::endl;
+        return _path_root + path;
+    }
+}
 string parserYAML::tagsToString(vector<std::string> &tags){
     string hdr = "";
     for(auto it : tags){
@@ -127,24 +143,18 @@ string parserYAML::tagsToString(vector<std::string> &tags){
 }
 void parserYAML::walkTree(string file){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     vector<string> hdrs = vector<string>();
     walkTreeR(n, hdrs, "");
 }
 void parserYAML::walkTree(string file, vector<string>& tags){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     walkTreeR(n, tags, "");
 }
 void parserYAML::walkTree(string file, vector<string>& tags, string hdr){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
     walkTreeR(n, tags, hdr);
 }
 void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
@@ -153,18 +163,13 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
         regex r("^@.*");
         if(regex_match(n.Scalar(), r)){
             string str = n.Scalar();
-            // cout << "SUBSTR " << str.substr(1,str.size() - 1);
             walkTree(str.substr(1,str.size() - 1), tags, hdr);
         }else{
-            // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬"));
-            // cout << "«»" << n.Scalar() << endl;
             _params.insert(pair<string,string>(hdr, n.Scalar()));
         }
         break;
     }
     case YAML::NodeType::Sequence : {
-        // cout << tags[tags.size() - 1] << "«»" << kv << endl;
-        // std::vector<double> vi = n.as<std::vector<double>>();
         string aux = parseSequence(n);
         _params.insert(pair<string,string>(hdr, aux));
         break;
@@ -217,7 +222,7 @@ void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){
         break;
     }
     default:
-        assert(1 == 0 && "Unsupported node Type at walkTreeR");
+        assert(1 == 0 && "Unsupported node Type at walkTreeR.");
         break;
     }
 }
@@ -226,11 +231,11 @@ void parserYAML::updateActiveName(string tag){
 }
 void parserYAML::parseFirstLevel(string file){
     YAML::Node n;
-    // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl;
-    if(not _relative_path) n = YAML::LoadFile(file);
-    else n = YAML::LoadFile(_path_root + file);
+    n = YAML::LoadFile(generatePath(file));
+
     YAML::Node n_config = n["config"];
     assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+    this->problem = n_config["problem"];
     for(const auto& kv : n_config["sensors"]){
         ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv};
         _paramsSens.push_back(pSensor);
@@ -243,7 +248,6 @@ void parserYAML::parseFirstLevel(string file){
         _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>(), kv[2].as<std::string>()}});
     }
     YAML::Node n_files = n["files"];
-    assert(n_files.Type() == YAML::NodeType::Sequence && "trying to parse files node but found a non-Sequence node");
     for(const auto& kv : n_files){
         _files.push_back(kv.Scalar());
     }
@@ -266,20 +270,26 @@ vector<string> parserYAML::getFiles(){
 vector<array<string, 3>> parserYAML::getCallbacks(){
     return this->_callbacks;
 }
+vector<array<string, 2>> parserYAML::getProblem(){
+    return vector<array<string, 2>>();
+}
 map<string,string> parserYAML::getParams(){
     map<string,string> rtn = _params;
     return rtn;
 }
 void parserYAML::parse(){
     this->parseFirstLevel(this->_file);
+
+    if(this->problem.Type() != YAML::NodeType::Undefined){
+        vector<string> tags = vector<string>();
+        this->walkTreeR(this->problem, tags , "problem");
+    }
     for(auto it : _paramsSens){
         vector<string> tags = vector<string>();
-        // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
         this->walkTreeR(it.n , tags , it._name);
     }
     for(auto it : _paramsProc){
         vector<string> tags = vector<string>();
-        // this->walkTreeR(it.n , tags , it._type + "/" + it._name);
         this->walkTreeR(it.n , tags , it._name);
     }
 }
@@ -295,8 +305,6 @@ map<string, string> parserYAML::fetchAsMap(YAML::Node n){
             break;
         }
         case YAML::NodeType::Sequence : {
-            // cout << tags[tags.size() - 1] << "«»" << kv << endl;
-            // std::vector<double> vi = n.as<std::vector<double>>();
             string aux = parseSequence(kv.second);
             m.insert(pair<string,string>(key, aux));
             break;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 49c5ae4f2d9474d34be5b83378f5b2517507cd62..c8ac508f52c3f70504d3c3fe2137573e426a5fe0 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -155,6 +155,20 @@ void FrameBase::removeStateBlocks()
     }
 }
 
+void FrameBase::setNonEstimated()
+{
+    // unregister if previously estimated
+    if (isKeyOrAux())
+        removeStateBlocks();
+
+    type_ = NON_ESTIMATED;
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
+}
+
 void FrameBase::setKey()
 {
     // register if previously not estimated
@@ -163,8 +177,11 @@ void FrameBase::setKey()
 
     // WOLF_DEBUG("Set Key", this->id());
     type_ = KEY;
-    getTrajectory()->sortFrame(shared_from_this());
-    getTrajectory()->updateLastFrames();
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
 }
 
 void FrameBase::setAux()
@@ -174,8 +191,11 @@ void FrameBase::setAux()
 
     // WOLF_DEBUG("Set Auxiliary", this->id());
     type_ = AUXILIARY;
-    getTrajectory()->sortFrame(shared_from_this());
-    getTrajectory()->updateLastFrames();
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
 }
 
 void FrameBase::fix()
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5bdfff041cc7a9bf8b2020b779736e61fac97da1..25dbf77343c40d83bc9a85960561fee198a4378b 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -3,17 +3,19 @@
 #include "core/factor/factor_base.h"
 #include "core/map/map_base.h"
 #include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/common/factory.h"
 #include "core/yaml/yaml_conversion.h"
 
 namespace wolf {
 
 unsigned int LandmarkBase::landmark_id_count_ = 0;
 
-    LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
-            NodeBase("LANDMARK", _type),
-            map_ptr_(),
-            state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
-            landmark_id_(++landmark_id_count_)
+LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
+        NodeBase("LANDMARK", _type),
+        map_ptr_(),
+        state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
+        landmark_id_(++landmark_id_count_)
 {
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
@@ -21,17 +23,6 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
 //    std::cout << "constructed  +L" << id() << std::endl;
 }
 
-    LandmarkBase::LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
-        NodeBase("LANDMARK", _type),
-        map_ptr_(_ptr),
-        state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
-        landmark_id_(++landmark_id_count_)
-    {
-        state_block_vec_[0] = _p_ptr;
-        state_block_vec_[1] = _o_ptr;
-
-        //    std::cout << "constructed  +L" << id() << std::endl;
-    }
 LandmarkBase::~LandmarkBase()
 {
     removeStateBlocks();
@@ -170,6 +161,7 @@ YAML::Node LandmarkBase::saveToYaml() const
     }
     return node;
 }
+
 void LandmarkBase::link(MapBasePtr _map_ptr)
 {
     if(_map_ptr)
@@ -192,4 +184,36 @@ FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
     return _fac_ptr;
 }
 
+LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
+{
+    unsigned int    id          = _node["id"]               .as< unsigned int     >();
+    Eigen::VectorXs pos         = _node["position"]         .as< Eigen::VectorXs  >();
+    bool            pos_fixed   = _node["position fixed"]   .as< bool >();
+
+    StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
+    StateBlockPtr ori_sb = nullptr;
+
+    if (_node["orientation"])
+    {
+        Eigen::VectorXs ori       = _node["orientation"].as< Eigen::VectorXs >();
+        bool            ori_fixed = _node["orientation fixed"].as< bool >();
+
+        if (ori.size() == 4)
+            ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
+        else
+            ori_sb = std::make_shared<StateBlock>(ori, ori_fixed);
+    }
+
+    LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("BASE", pos_sb, ori_sb);
+    lmk->setId(id);
+
+    return lmk;
+}
+
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_ahp = LandmarkFactory::get().registerCreator("BASE", LandmarkBase::create);
+}
+
 } // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 204fc7b8ffd3d1b1a8c49030c74c0e803e11b7c4..bff3d2a105ce9c4dc44050d86bb8c25b3668d739 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -35,9 +35,6 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
 
 void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
 {
-    for (auto lmk : _landmark_list)
-        addLandmark(lmk);
-
     //TEMPORARY FIX, should be made compliant with the new emplace methodology
 	LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
    for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
@@ -61,7 +58,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
     {
         YAML::Node lmk_node = map["landmarks"][i];
         LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node);
-        addLandmark(lmk_ptr);
+        lmk_ptr->link(shared_from_this());
     }
 
 }
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 62c035cf0c7adfc58f2a1ee8c23ac7483d5f0564..fa78a9e38aea5c2ba26cbd4ee3f5a54531dd6685 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -72,13 +72,15 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
     p->setup();
     return p->shared_from_this();
 }
-ProblemPtr Problem::autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file)
+ProblemPtr Problem::autoSetup(const std::string& _yaml_file)
 {
-    auto p = Problem::create(_frame_structure, _dim);
     // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml";
     parserYAML parser = parserYAML(_yaml_file);
     parser.parse();
     paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
+    std::string frame_structure = server.getParam<std::string>("problem/frame structure", "PO");
+    int dim = server.getParam<int>("problem/dimension", "2");
+    auto p = Problem::create(frame_structure, dim);
     // cout << "PRINTING SERVER MAP" << endl;
     // server.print();
     // cout << "-----------------------------------" << endl;
@@ -205,7 +207,8 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
         throw std::runtime_error("Sensor not found. Cannot bind Processor.");
     ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr);
     prc_ptr->configure(sen_ptr);
-    sen_ptr->addProcessor(prc_ptr);
+    prc_ptr->link(sen_ptr);
+    // sen_ptr->addProcessor(prc_ptr);
 
     // setting the origin in all processor motion if origin already setted
     if (prc_ptr->isMotion() && prior_is_set_)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index d7c7d8d8f6cfd588d9b03ac9782f2d6ef583cb6b..04f5f04cba9d712740151f9c445af4eb25211035 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -54,7 +54,7 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _
 {
     WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
     if (_keyframe_ptr != nullptr)
-        kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
+        buffer_pack_kf_.add(_keyframe_ptr,_time_tol_other);
 }
 
 void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
@@ -100,7 +100,7 @@ void ProcessorBase::remove()
     }
 /////////////////////////////////////////////////////////////////////////////////////////
 
-void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
+void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
 {
     TimeStamp time_stamp = _key_frame->getTimeStamp();
     PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
@@ -108,12 +108,12 @@ void PackKeyFrameBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time
     Buffer::add(kfpack, time_stamp);
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
 {
     if (container_.empty())
         return nullptr;
 
-    PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
+    BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp);
 
     // remove packs corresponding to removed KFs (keeping the next iterator in post)
     while (post != container_.end() && post->second->key_frame->isRemoving())
@@ -128,7 +128,7 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
 
     if (prev_exists)
     {
-        PackKeyFrameBuffer::Iterator prev = std::prev(post);
+        BufferPackKeyFrame::Iterator prev = std::prev(post);
 
         bool prev_ok = doubleCheckTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
 
@@ -151,12 +151,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
 
     return nullptr;
 }
-PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
 {
     return selectPack(_capture->getTimeStamp(), _time_tolerance);
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
 {
     // remove packs corresponding to removed KFs
     while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
@@ -180,12 +180,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time
 
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
 {
     return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
 }
 
-void PackKeyFrameBuffer::print(void)
+void BufferPackKeyFrame::print(void)
 {
     std::cout << "[ ";
     for (auto iter : container_)
diff --git a/src/processor/processor_loopclosure_base2.cpp b/src/processor/processor_loopclosure_base2.cpp
index a56e0fe0bdba0c602f0a1da7161c3422679f00e4..f6f0848ad8b7c162e3a7124b4d497dcc0e235297 100644
--- a/src/processor/processor_loopclosure_base2.cpp
+++ b/src/processor/processor_loopclosure_base2.cpp
@@ -54,10 +54,10 @@ void ProcessorLoopClosureBase2::processLoopClosure()
  */
 std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosureBase2::selectPairKC()
 {
-    std::map<TimeStamp,PackKeyFramePtr> kf_container = kf_pack_buffer_.getContainer();
+    std::map<TimeStamp,PackKeyFramePtr> kf_container = buffer_pack_kf_.getContainer();
     if (kf_container.empty()){
         return std::make_pair(nullptr, nullptr);};
-    PackKeyFrameBuffer::Iterator kf_it;
+    BufferPackKeyFrame::Iterator kf_it;
     for (kf_it=kf_container.begin(); kf_it!=kf_container.end(); ++kf_it)
     {
         CaptureBasePtr cap_ptr = buffer_capture_.select(kf_it->first, kf_it->second->time_tolerance);
@@ -65,7 +65,7 @@ std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosureBase2::selectPairKC()
         {
             // clear the buffers :
             buffer_capture_.removeUpTo(cap_ptr->getTimeStamp());
-            kf_pack_buffer_.removeUpTo(kf_it->first);
+            buffer_pack_kf_.removeUpTo(kf_it->first);
             // return the KF-Cap pair :
             return std::make_pair(kf_it->second->key_frame, cap_ptr);
         };
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index bb2812f75847545fde2813640d9f8413fa9fa413..02b975499940b2e01d0e477f843d8d2b496a1fda 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -92,7 +92,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
 
     PackKeyFramePtr pack = computeProcessingStep();
     if (pack)
-        kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+        buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
 
     switch(processing_step_)
     {
@@ -656,11 +656,11 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
         throw std::runtime_error("ProcessorMotion received data before being initialized.");
     }
 
-    PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+    PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
 
     if (pack)
     {
-        if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+        if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
         {
             WOLF_WARN("||*||");
             WOLF_INFO(" ... It seems you missed something!");
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7c4f2e6e5fe1ab437ece2beedee0ac50d2b0e8cd..ad2c73b62f0c5a91601f59272aae2355b9b313d6 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -52,8 +52,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
     {
         case FIRST_TIME_WITH_PACK :
         {
-            PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
-            kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+            PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
+            buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
 
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
@@ -97,7 +97,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         case SECOND_TIME_WITH_PACK :
         {
         	// No-break case only for debug. Next case will be executed too.
-            PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
+            PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
         } // @suppress("No break at end of case")
         case SECOND_TIME_WITHOUT_PACK :
@@ -124,8 +124,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         }
         case RUNNING_WITH_PACK :
         {
-            PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
-            kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
+            PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
+            buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
 
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
@@ -182,9 +182,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 // process
                 processNew(params_tracker_->max_new_features);
 
-                // Set state to the keyframe
-                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-
                 // Establish factors
                 establishFactors();
 
@@ -211,9 +208,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
                 frm->addCapture(incoming_ptr_);
 
-                // Set state to the keyframe
-                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-
                 // Establish factors
                 establishFactors();
 
@@ -271,7 +265,7 @@ void ProcessorTracker::computeProcessingStep()
     {
         case FIRST_TIME :
 
-            if (kf_pack_buffer_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
+            if (buffer_pack_kf_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
                 processing_step_ = FIRST_TIME_WITH_PACK;
             else // ! last && ! pack(incoming)
                 processing_step_ = FIRST_TIME_WITHOUT_PACK;
@@ -279,7 +273,7 @@ void ProcessorTracker::computeProcessingStep()
 
         case SECOND_TIME :
 
-            if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+            if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
                 processing_step_ = SECOND_TIME_WITH_PACK;
             else
                 processing_step_ = SECOND_TIME_WITHOUT_PACK;
@@ -288,7 +282,7 @@ void ProcessorTracker::computeProcessingStep()
         case RUNNING :
         default :
 
-            if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+            if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
             {
                 if (last_ptr_->getFrame()->isKey())
                 {
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index dbcc3646b0954a4e34da071ba7b34c878d79e265..fc923797005a5a5efd5e01c1e88473b6b595d82e 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -144,6 +144,9 @@ void ProcessorTrackerFeature::resetDerived()
 
 void ProcessorTrackerFeature::establishFactors()
 {
+    if (origin_ptr_ == last_ptr_)
+        return;
+
     TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
 
     for (auto const & pair_trkid_pair : matches_origin_last)
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index c6776995796c55b82a70acd396ea0dca63237190..999f8620792dd83a7e34cb295ec4cf32049568ad 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -11,19 +11,19 @@
 namespace wolf
 {
 
-unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
-
 unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in,
                                                          FeatureBasePtrList& _features_incoming_out,
                                                          FeatureMatchMap& _feature_correspondences)
 {
     WOLF_INFO("tracking " , _features_last_in.size() , " features...");
 
+    auto count = 0;
     for (auto feat_in : _features_last_in)
     {
-        if (++count_ % 3 == 2) // lose one every 3 tracks
+        if (count < params_tracker_feature_dummy_->n_tracks_lost) // lose first ntrackslost tracks
         {
             WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!");
+            count++;
         }
         else
         {
@@ -63,9 +63,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
     // detecting new features
     for (unsigned int i = 0; i < max_features; i++)
     {
-        n_feature_++;
-        FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
-                                                         n_feature_* Eigen::Vector1s::Ones(),
+        FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                         Eigen::Vector1s::Ones(),
                                                          Eigen::MatrixXs::Ones(1, 1)));
         _features_incoming_out.push_back(ftr);
 
@@ -78,14 +77,36 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_fea
 }
 
 FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature_ptr,
-                                                                 FeatureBasePtr _feature_other_ptr)
+                                                         FeatureBasePtr _feature_other_ptr)
 {
     WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
                , " with origin feature " , _feature_other_ptr->id() );
 
-    auto ctr = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
+    auto fac = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr, shared_from_this());
 
-    return ctr;
+    return fac;
 }
 
+ProcessorBasePtr ProcessorTrackerFeatureDummy::create(const std::string& _unique_name,
+                                                      const ProcessorParamsBasePtr _params,
+                                                      const SensorBasePtr)
+{
+    ProcessorParamsTrackerFeatureDummyPtr params = std::static_pointer_cast<ProcessorParamsTrackerFeatureDummy>(_params);
+
+    // if cast failed use default value
+    if (params == nullptr)
+        params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
+
+    ProcessorTrackerFeatureDummyPtr prc_ptr = std::make_shared<ProcessorTrackerFeatureDummy>(params);
+    prc_ptr->setName(_unique_name);
+
+    return prc_ptr;
+}
+
+} // namespace wolf
+
+// Register in the ProcessorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
 } // namespace wolf
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 1706985724d3d61d388a2d70e83fb87c12483a4f..c55b21dc981c000ccf508b82ecf1f6842a1285c0 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -63,7 +63,8 @@ unsigned int ProcessorTrackerLandmark::processKnown()
     // Find landmarks in incoming_ptr_
     FeatureBasePtrList known_features_list_incoming;
     unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
-                                                 known_features_list_incoming, matches_landmark_from_incoming_);
+                                   known_features_list_incoming,
+                                   matches_landmark_from_incoming_);
     // Append found incoming features
     incoming_ptr_->addFeatureList(known_features_list_incoming);
 
@@ -131,18 +132,6 @@ void ProcessorTrackerLandmark::createNewLandmarks()
     }
 }
 
-// unsigned int ProcessorTrackerLandmark::processKnown()
-// {
-//     // Find landmarks in incoming_ptr_
-//     FeatureBasePtrList known_features_list_incoming;
-//     unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
-//                                                  known_features_list_incoming, matches_landmark_from_incoming_);
-//     // Append found incoming features
-//     incoming_ptr_->addFeatureList(known_features_list_incoming);
-
-//     return n;
-// }
-
 void ProcessorTrackerLandmark::establishFactors()
 {
     // Loop all features in last_ptr_
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61540a8afec1b4c204e59b92fca645efde1cd4ca
--- /dev/null
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -0,0 +1,110 @@
+/**
+ * \file processor_tracker_landmark_dummy.cpp
+ *
+ *  Created on: Apr 12, 2016
+ *      \author: jvallve
+ */
+
+#include "core/processor/processor_tracker_landmark_dummy.h"
+#include "core/landmark/landmark_base.h"
+#include "core/factor/factor_landmark_dummy.h"
+
+namespace wolf
+{
+
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
+        ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark_dummy),
+        params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
+{
+    //
+
+}
+
+ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
+{
+    //
+}
+
+unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+                                                          FeatureBasePtrList& _features_incoming_out,
+                                                          LandmarkMatchMap& _feature_landmark_correspondences)
+{
+    std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks"  << std::endl;
+    std::cout << "\t\t"  << _landmarks_in.size() << " landmarks..." << std::endl;
+
+    // loosing the track of the first landmark_idx_non_visible_ features
+    auto prev_found = matches_landmark_from_last_.size();
+    if (prev_found == 0) prev_found = _landmarks_in.size();
+    auto count = 0;
+    for (auto landmark_in_ptr : _landmarks_in)
+    {
+        if (count < prev_found - params_tracker_landmark_dummy_->n_landmarks_lost) // lose last n_landmarks_lost landmarks
+        {
+            FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                             Eigen::Vector1s::Ones(),
+                                                             Eigen::MatrixXs::Ones(1, 1)));
+            _features_incoming_out.push_back(ftr);
+            _feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
+            std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl;
+        }
+        else
+        {
+            WOLF_INFO("landmark: " , landmark_in_ptr->id() , " lost!");
+        }
+        count++;
+    }
+    return _features_incoming_out.size();
+}
+
+bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
+{
+    std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
+    bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_landmark_dummy_->min_features_for_keyframe;
+    std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
+    return vote;
+}
+
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+{
+    std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
+
+    unsigned int max_features = _max_features;
+
+    if (max_features == -1)
+    {
+        max_features = 10;
+        WOLF_INFO("max_features unlimited, setting it to " , max_features);
+    }
+    WOLF_INFO("Detecting " , _max_features , " new features..." );
+
+    // detecting new features
+    for (unsigned int i = 0; i < max_features; i++)
+    {
+        FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                         Eigen::Vector1s::Ones(),
+                                                         Eigen::MatrixXs::Ones(1, 1)));
+        _features_incoming_out.push_back(ftr);
+
+        WOLF_INFO("feature " , ftr->id() , " detected!" );
+    }
+
+    WOLF_INFO(_features_incoming_out.size() , " features detected!");
+
+    return _features_incoming_out.size();
+}
+
+LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
+{
+    //std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
+    return std::make_shared<LandmarkBase>("BASE", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+}
+
+FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+{
+    std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
+    std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
+    std::cout << "\t\tlandmark "<< _landmark_ptr->id() << std::endl;
+    return std::make_shared<FactorLandmarkDummy>(_feature_ptr, _landmark_ptr, shared_from_this());
+}
+
+} //namespace wolf
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index af0bff7f350f7a317548de3b9048fad4874a8bfd..394fa88964d46e1a0d0e1632a058a124cb4ecfdc 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -32,8 +32,8 @@ Track TrackMatrix::track(size_t _track_id)
 
 Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
 {
-    if (_capture && snapshots_.count(_capture->id()) > 0)
-        return snapshots_.at(_capture->id());
+    if (_capture && snapshots_.count(_capture) > 0)
+        return snapshots_.at(_capture);
     else
         return Snapshot();
 }
@@ -52,7 +52,7 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr
     if (_cap != _ftr->getCapture())
         _ftr->setCapture(_cap);
     tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
-    snapshots_[_cap->id()].emplace(_track_id, _ftr);        // will create new snapshot if _cap_id   is not present
+    snapshots_[_cap].emplace(_track_id, _ftr);        // will create new snapshot if _cap_id   is not present
 }
 
 void TrackMatrix::remove(size_t _track_id)
@@ -62,10 +62,11 @@ void TrackMatrix::remove(size_t _track_id)
     {
         for (auto const& pair_time_ftr : tracks_.at(_track_id))
         {
-            SizeStd cap_id = pair_time_ftr.second->getCapture()->id();
-            snapshots_.at(cap_id).erase(_track_id);
-            if (snapshots_.at(cap_id).empty())
-                snapshots_.erase(cap_id);
+            CaptureBasePtr cap = pair_time_ftr.second->getCapture();
+            snapshots_.at(cap).erase(_track_id);
+            if (snapshots_.at(cap).empty())
+                snapshots_.erase(cap);
+
         }
 
         // Remove track
@@ -76,10 +77,10 @@ void TrackMatrix::remove(size_t _track_id)
 void TrackMatrix::remove(CaptureBasePtr _cap)
 {
     // remove snapshot features from all tracks
-    if (snapshots_.count(_cap->id()))
+    if (snapshots_.count(_cap))
     {
         TimeStamp ts = _cap->getTimeStamp();
-        for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
+        for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
         {
             SizeStd trk_id = pair_trkid_ftr.first;
             tracks_.at(trk_id).erase(ts);
@@ -88,7 +89,7 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
         }
 
         // remove snapshot
-        snapshots_.erase(_cap->id());
+        snapshots_.erase(_cap);
     }
 }
 
@@ -99,13 +100,13 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
     {
         if (auto cap = _ftr->getCapture())
         {
-            tracks_   .at(_ftr->trackId()).erase(cap->getTimeStamp());
+            tracks_    .at(_ftr->trackId()).erase(cap->getTimeStamp());
             if (tracks_.at(_ftr->trackId()).empty())
                 tracks_.erase(_ftr->trackId());
 
-            snapshots_.at(cap->id())      .erase(_ftr->trackId());
-            if (snapshots_.at(cap->id()).empty())
-                snapshots_.erase(cap->id());
+            snapshots_.at(cap)      .erase(_ftr->trackId());
+            if (snapshots_.at(cap).empty())
+                snapshots_.erase(cap);
         }
     }
 }
@@ -151,8 +152,8 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(size_t _track_id)
 std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
 {
     std::list<FeatureBasePtr> lst;
-    if (snapshots_.count(_cap->id()))
-        for (auto const& pair_trkid_ftr : snapshots_.at(_cap->id()))
+    if (snapshots_.count(_cap))
+        for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
             lst.push_back(pair_trkid_ftr.second);
     return lst;
 }
@@ -194,4 +195,23 @@ CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
     return firstFeature(_track_id)->getCapture();
 }
 
+Track TrackMatrix::trackAtKeyframes(size_t _track_id)
+{
+    // We assemble a track_kf on the fly by checking each capture's frame.
+    if (tracks_.count(_track_id))
+    {
+        Track track_kf;
+        for (auto& pair_ts_ftr : tracks_.at(_track_id))
+        {
+            auto& ts  = pair_ts_ftr.first;
+            auto& ftr = pair_ts_ftr.second;
+            if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey())
+                track_kf[ts] = ftr;
+        }
+        return track_kf;
+    }
+    else
+        return Track();
+}
+
 }
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index c633261ade4a3835e38969eb122d59d5be014928..3ad6e39bcd7a6b44013b48c4167e5bada37e1216 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -406,7 +406,6 @@ void SensorBase::setProblem(ProblemPtr _problem)
 
 void SensorBase::link(HardwareBasePtr _hw_ptr)
 {
-    std::cout << "Linking SensorBase" << std::endl;
     if(_hw_ptr)
     {
         this->setHardware(_hw_ptr);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c0d47dea04b20e26ed26cf685b68f169a073bdab..2bdc605a30dc5f70f4028c6c5007f8b731b48467 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -76,6 +76,10 @@ target_link_libraries(gtest_frame_base ${PROJECT_NAME})
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
 target_link_libraries(gtest_local_param ${PROJECT_NAME})
 
+# Logging test
+wolf_add_gtest(gtest_logging gtest_logging.cpp)
+target_link_libraries(gtest_logging ${PROJECT_NAME})
+
 # MotionBuffer class test
 wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
 target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
@@ -162,28 +166,41 @@ target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
 wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
 target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
 
-
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
 target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
 
+# Map yaml save/load test
+wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
+target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
+
 # Parameter prior test
 wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
 target_link_libraries(gtest_param_prior ${PROJECT_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})
 
-
 # ProcessorMotion in 2D
 wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
 target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
 
 # ProcessorOdom3D class test
-wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
-target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
+wolf_add_gtest(gtest_processor_odom_3D gtest_processor_odom_3D.cpp)
+target_link_libraries(gtest_processor_odom_3D ${PROJECT_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})
+
+# 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})
+
+# yaml conversions
+wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
+target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
 
 # ------- Now Core classes Serialization ----------
 
diff --git a/test/gtest_logging.cpp b/test/gtest_logging.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec6df4414d1934fd4c1f4c3dd989639848dbad82
--- /dev/null
+++ b/test/gtest_logging.cpp
@@ -0,0 +1,42 @@
+/**
+ * \file test_wolf_logging.cpp
+ *
+ * Created on: Oct 28, 2016
+ * \author: Jeremie Deray
+ */
+
+#include "core/common/wolf.h"
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+TEST(logging, info)
+{
+    WOLF_INFO("test info ", 5, " ", 0.123);
+}
+
+TEST(logging, warn)
+{
+    WOLF_WARN("test warn ", 5, " ", 0.123);
+}
+
+TEST(logging, error)
+{
+    WOLF_ERROR("test error ", 5, " ", 0.123);
+}
+
+TEST(logging, trace)
+{
+    WOLF_TRACE("test trace ", 5, " ", 0.123);
+}
+
+TEST(logging, debug)
+{
+    WOLF_DEBUG("test debug ", 5, " ", 0.123);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..60ac575e2a13591b22a8ce0c0217d049124cc9e2
--- /dev/null
+++ b/test/gtest_map_yaml.cpp
@@ -0,0 +1,291 @@
+/**
+ * \file test_map_yaml.cpp
+ *
+ *  Created on: Jul 27, 2016
+ *      \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "core/map/map_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+#include "core/yaml/yaml_conversion.h"
+
+#include <iostream>
+using namespace wolf;
+
+TEST(MapYaml, save_2D)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    Eigen::Vector2s p1, p2, p3;
+    Eigen::Vector1s o2, o3;
+    p1 << 1, 2;
+    p2 << 3, 4;
+    p3 << 5, 6;
+    o2 << 2;
+    o3 << 3;
+
+    StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
+    StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
+    StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
+    StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
+    StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
+
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb);
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, o2_sb);
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, o3_sb);
+
+    std::string wolf_root = _WOLF_ROOT_DIR;
+    std::string map_path  = wolf_root + "/test/yaml";
+
+    // Check Problem functions
+    std::string filename  = map_path + "/map_2D_problem.yaml";
+    std::cout << "Saving map to file: " << filename << std::endl;
+    problem->saveMap(filename, "2D map saved from Problem::saveMap()");
+
+    // Check Map functions
+    filename = map_path + "/map_2D_map.yaml";
+    std::cout << "Saving map to file: " << filename << std::endl;
+    problem->getMap()->save(filename, "2D map saved from Map::save()");
+}
+
+TEST(MapYaml, load_2D)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    std::string wolf_root = _WOLF_ROOT_DIR;
+    std::string map_path  = wolf_root + "/test/yaml";
+
+    // Check Problem functions
+    std::string filename  = map_path + "/map_2D_problem.yaml";
+    std::cout << "Loading map to file: " << filename << std::endl;
+    problem->loadMap(filename);
+
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+    for (auto lmk : problem->getMap()->getLandmarkList())
+    {
+        if (lmk->id() == 1)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() == nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<1,2).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+        }
+        else if (lmk->id() == 2)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<3,4).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+            ASSERT_FALSE(lmk->getO()->isFixed());
+        }
+        else if (lmk->id() == 3)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<5,6).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<3).finished(), 1e-12);
+            ASSERT_TRUE(lmk->getP()->isFixed());
+            ASSERT_TRUE(lmk->getO()->isFixed());
+        }
+    }
+
+    // empty the map
+    {
+        auto lmk_list = problem->getMap()->getLandmarkList();
+        for (auto lmk : lmk_list)
+            lmk->remove();
+    }
+    ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+
+    // Check Map functions
+    filename  = map_path + "/map_2D_map.yaml";
+    std::cout << "Loading map to file: " << filename << std::endl;
+    problem->getMap()->load(filename);
+
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+    for (auto lmk : problem->getMap()->getLandmarkList())
+    {
+        if (lmk->id() == 1)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() == nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<1,2).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+        }
+        else if (lmk->id() == 2)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<3,4).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+            ASSERT_FALSE(lmk->getO()->isFixed());
+        }
+        else if (lmk->id() == 3)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<5,6).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<3).finished(), 1e-12);
+            ASSERT_TRUE(lmk->getP()->isFixed());
+            ASSERT_TRUE(lmk->getO()->isFixed());
+        }
+    }
+}
+TEST(MapYaml, save_3D)
+{
+    ProblemPtr problem = Problem::create("PO", 3);
+
+    Eigen::Vector3s p1, p2, p3;
+    Eigen::Vector4s q2, q3;
+    p1 << 1, 2, 3;
+    p2 << 4, 5, 6;
+    p3 << 7, 8, 9;
+    q2 << 0, 1, 0, 0;
+    q3 << 0, 0, 1, 0;
+
+    auto p1_sb = std::make_shared<StateBlock>(p1);
+    auto p2_sb = std::make_shared<StateBlock>(p2);
+    auto q2_sb = std::make_shared<StateQuaternion>(q2);
+    auto p3_sb = std::make_shared<StateBlock>(p3, true);
+    auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
+
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb);
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, q2_sb);
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, q3_sb);
+
+    std::string wolf_root = _WOLF_ROOT_DIR;
+    std::string map_path  = wolf_root + "/test/yaml";
+
+    // Check Problem functions
+    std::string filename  = map_path + "/map_3D_problem.yaml";
+    std::cout << "Saving map to file: " << filename << std::endl;
+    problem->saveMap(filename, "3D map saved from Problem::saveMap()");
+
+    // Check Map functions
+    filename = map_path + "/map_3D_map.yaml";
+    std::cout << "Saving map to file: " << filename << std::endl;
+    problem->getMap()->save(filename, "3D map saved from Map::save()");
+}
+
+TEST(MapYaml, load_3D)
+{
+    ProblemPtr problem = Problem::create("PO", 3);
+
+    std::string wolf_root = _WOLF_ROOT_DIR;
+    std::string map_path  = wolf_root + "/test/yaml";
+
+    // Check Problem functions
+    std::string filename  = map_path + "/map_3D_problem.yaml";
+    std::cout << "Loading map to file: " << filename << std::endl;
+    problem->loadMap(filename);
+
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+    for (auto lmk : problem->getMap()->getLandmarkList())
+    {
+        if (lmk->id() == 1)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() == nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<1,2,3).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+        }
+        else if (lmk->id() == 2)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<4,5,6).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,1,0,0).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+            ASSERT_FALSE(lmk->getO()->isFixed());
+            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+        }
+        else if (lmk->id() == 3)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<7,8,9).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,0,1,0).finished(), 1e-12);
+            ASSERT_TRUE(lmk->getP()->isFixed());
+            ASSERT_TRUE(lmk->getO()->isFixed());
+            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+        }
+    }
+
+    // empty the map
+    {
+        auto lmk_list = problem->getMap()->getLandmarkList();
+        for (auto lmk : lmk_list)
+            lmk->remove();
+    }
+    ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+
+    // Check Map functions
+    filename  = map_path + "/map_3D_map.yaml";
+    std::cout << "Loading map to file: " << filename << std::endl;
+    problem->getMap()->load(filename);
+
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+
+    for (auto lmk : problem->getMap()->getLandmarkList())
+    {
+        if (lmk->id() == 1)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() == nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<1,2,3).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+        }
+        else if (lmk->id() == 2)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<4,5,6).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,1,0,0).finished(), 1e-12);
+            ASSERT_FALSE(lmk->getP()->isFixed());
+            ASSERT_FALSE(lmk->getO()->isFixed());
+            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+        }
+        else if (lmk->id() == 3)
+        {
+            ASSERT_TRUE(lmk->getP() != nullptr);
+            ASSERT_TRUE(lmk->getO() != nullptr);
+            ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<7,8,9).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,0,1,0).finished(), 1e-12);
+            ASSERT_TRUE(lmk->getP()->isFixed());
+            ASSERT_TRUE(lmk->getO()->isFixed());
+            ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+        }
+    }
+}
+
+TEST(MapYaml, remove_map_files)
+{
+    std::string wolf_root = _WOLF_ROOT_DIR;
+    std::string map_path  = wolf_root + "/test/yaml";
+
+    std::string filename  = map_path + "/map_2D_problem.yaml";
+    ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+    filename  = map_path + "/map_2D_map.yaml";
+    ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+    filename  = map_path + "/map_3D_problem.yaml";
+    ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+    filename  = map_path + "/map_3D_map.yaml";
+    ASSERT_TRUE(std::remove(filename.c_str()) == 0);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 6597f0f53207c98759e882a96221a2ac0fcb00d0..1b1f1ffb61ded8028dfbe7814f6f813f76793334 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -26,7 +26,7 @@ class BufferPackKeyFrameTest : public testing::Test
 {
     public:
 
-        PackKeyFrameBuffer pack_kf_buffer;
+        BufferPackKeyFrame pack_kf_buffer;
         FrameBasePtr f10, f20, f21, f28;
         Scalar tt10, tt20, tt21, tt28;
         TimeStamp timestamp;
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index 3e83f8f89955a57342a0ec5247afb63303de69ce..dc5f5a5aa40962abba31b3e21ccdaa3aeb2d928a 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -18,7 +18,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParamsServer, Default)
 {
-  auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+  auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
   paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
   EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 97451a6060ccd3adb43fd990c929c9cfee7b8dbc..d3ba3a40587e1e732cccf4d1a6c0d39e58a0207f 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -17,7 +17,7 @@ parserYAML parse(string _file, string _path_root)
 
 TEST(ParserYAML, RegularParse)
 {
-  auto parser = parse("/test/yaml/params1.yaml", wolf_root);
+  auto parser = parse("test/yaml/params1.yaml", wolf_root);
   auto params = parser.getParams();
   // for(auto it : params)
   //   cout << it.first << " %% " << it.second << endl;
@@ -26,16 +26,23 @@ TEST(ParserYAML, RegularParse)
 }
 TEST(ParserYAML, ParseMap)
 {
-  auto parser = parse("/test/yaml/params2.yaml", wolf_root);
+  auto parser = parse("test/yaml/params2.yaml", wolf_root);
   auto params = parser.getParams();
   EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
 }
 TEST(ParserYAML, JumpFile)
 {
-  auto parser = parse("/test/yaml/params3.yaml", wolf_root);
+  auto parser = parse("test/yaml/params3.yaml", wolf_root);
   auto params = parser.getParams();
-  EXPECT_EQ(params["my_proc_test/max_buff_length"], "100");
-  EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false");
+  EXPECT_EQ(params["my_proc_test/extern params/max_buff_length"], "100");
+  EXPECT_EQ(params["my_proc_test/extern params/voting_active"], "false");
+}
+TEST(ParserYAML, ProblemConfig)
+{
+  auto parser = parse("test/yaml/params2.yaml", wolf_root);
+  auto params = parser.getParams();
+  EXPECT_EQ(params["problem/frame structure"], "POV");
+  EXPECT_EQ(params["problem/dimension"], "2");
 }
 int main(int argc, char **argv)
 {
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 44fac86d12c9134180c83896d6906ee954a65abf..d36a14461c139e60be272f5f54c883e77ff35999 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -114,13 +114,7 @@ TEST(Problem, Installers)
     SensorBasePtr    S = P->installSensor   ("ODOM 3D", "odometer",        xs,         wolf_root + "/test/yaml/sensor_odom_3D.yaml");
 
     // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
-    ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
-    params->time_tolerance = 0.1;
-    params->max_new_features = 5;
-    params->min_features_for_keyframe = 10;
-    // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
-    auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(S, ProcessorTrackerFeatureDummy(params));
-    // S->addProcessor(pt);
+    auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
 
     // check motion processor IS NOT set
     ASSERT_FALSE(P->getProcessorMotion());
@@ -250,13 +244,8 @@ TEST(Problem, StateBlocks)
     SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
 
-    ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
-    params->time_tolerance            = 0.1;
-    params->max_new_features          = 5;
-    params->min_features_for_keyframe = 10;
-    
-    auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
-    ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+    auto pt = P->installProcessor("TRACKER FEATURE DUMMY",  "dummy",            "odometer");
+    auto pm = P->installProcessor("ODOM 3D",                "odom integrator",  "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
 
     // 2 state blocks, estimated
     auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
@@ -309,11 +298,9 @@ TEST(Problem, Covariances)
     params->time_tolerance            = 0.1;
     params->max_new_features          = 5;
     params->min_features_for_keyframe = 10;
-    // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
-    auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
 
-    // St->addProcessor(pt);
-    ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+    auto pt = P->installProcessor("TRACKER FEATURE DUMMY",  "dummy",            "odometer");
+    auto pm = P->installProcessor("ODOM 3D",                "odom integrator",  "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index cff4bc083e1fa349e7986df8d6e4af1bb56b15f6..05ce862fddbdfb76937205e65d49471b0f324795 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -38,28 +38,18 @@ TEST(ProcessorBase, KeyFrameCallback)
     ProblemPtr problem = Problem::create("PO", 2);
 
     // Install tracker (sensor and processor)
-    // SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
-    //                                                  std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
-    //                                                  std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
-    auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(), "FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+    auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+                                                    "SENSOR BASE",
+                                                    std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
                                                     std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
                                                     std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-    ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
-    params->time_tolerance            = dt/2;
-    params->max_new_features          = 5;
-    params->min_features_for_keyframe = 5;
-    // shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params);
-    auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, params);
-
-    // problem->addSensor(sens_trk);
-    // sens_trk->addProcessor(proc_trk);
+    auto proc_trk = problem->installProcessor("TRACKER FEATURE DUMMY",  "dummy", sens_trk);
 
     // Install odometer (sensor and processor)
     SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
     ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
     proc_odo_params->time_tolerance = dt/2;
-    ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odometer", sens_odo, proc_odo_params);
+    ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odom processor", sens_odo, proc_odo_params);
 
     std::cout << "sensor & processor created and added to wolf problem" << std::endl;
 
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_processor_odom_3D.cpp
similarity index 100%
rename from test/gtest_odom_3D.cpp
rename to test/gtest_processor_odom_3D.cpp
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..02e24d90b19dc6a9537ccfbb3a7434887ce6c48e
--- /dev/null
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -0,0 +1,314 @@
+
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/sensor_factory.h"
+#include "core/processor/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummyDummy);
+
+class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
+{
+	public:
+
+        ProcessorTrackerFeatureDummyDummy(ProcessorParamsTrackerFeatureDummyPtr& _params) :
+            ProcessorTrackerFeatureDummy(_params){}
+
+		void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; }
+		void setInc(CaptureBasePtr _incoming_ptr){ incoming_ptr_ = _incoming_ptr; }
+
+		unsigned int callProcessKnown(){ return this->processKnown(); }
+
+		unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); }
+
+        unsigned int callDetectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out){ return this->detectNewFeatures(_max_features, _features_incoming_out); }
+
+        unsigned int callTrackFeatures(const FeatureBasePtrList& _features_last_in,
+                                       FeatureBasePtrList& _features_incoming_out,
+                                       FeatureMatchMap& _feature_correspondences){ return this->trackFeatures(_features_last_in, _features_incoming_out, _feature_correspondences); }
+
+        FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return this->createFactor(_feature_ptr, _feature_other_ptr); }
+
+        void callEstablishFactors(){ this->establishFactors();}
+
+		TrackMatrix getTrackMatrix(){ return track_matrix_; }
+
+        FeatureMatchMap    getMatchesLastFromIncoming() { return matches_last_from_incoming_; }
+
+        void callReset()
+        {
+            this->resetDerived();
+            origin_ptr_ = last_ptr_;
+            last_ptr_   = incoming_ptr_;
+            incoming_ptr_ = nullptr;
+        };
+};
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class ProcessorTrackerFeatureDummyTest : public testing::Test
+{
+    public:
+        ProblemPtr problem;
+        SensorBasePtr sensor;
+        ProcessorParamsTrackerFeatureDummyPtr params;
+        ProcessorTrackerFeatureDummyDummyPtr processor;
+
+        virtual void SetUp()
+        {
+            // Wolf problem
+            problem = Problem::create("PO", 2);
+
+            // Install camera
+            sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
+
+            // Install processor
+            params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
+            params->max_new_features = 10;
+            params->min_features_for_keyframe = 7;
+            params->time_tolerance = 0.25;
+            params->voting_active = true;
+            params->n_tracks_lost = 1; // 1 (the first) track is lost each time trackFeatures is called
+            processor  = std::make_shared<ProcessorTrackerFeatureDummyDummy>(params);
+            processor->link(sensor);
+        }
+};
+
+TEST_F(ProcessorTrackerFeatureDummyTest, installProcessor)
+{
+    ASSERT_EQ(processor->getProblem(), problem);
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, callDetectNewFeatures)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // demo callDetectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+    ASSERT_EQ(n_feat, feat_list.size());
+    ASSERT_EQ(n_feat, params->max_new_features);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, trackFeatures)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    //fill feat_last list
+    processor->callDetectNewFeatures(params->max_new_features, feat_list);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    //test trackFeatures
+    FeatureBasePtrList feat_list_out;
+    FeatureMatchMap feat_correspondance;
+    processor->callTrackFeatures(feat_list, feat_list_out, feat_correspondance);
+    ASSERT_EQ(feat_list_out.size(), feat_correspondance.size());
+    ASSERT_EQ(feat_list.size(), feat_list_out.size()+1); // one of each 10 tracks is lost
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, processNew)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    auto n_new_feat = processor->callProcessNew(10); // detect 10 features
+
+    ASSERT_EQ(n_new_feat, 10); // detected 10 features
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 track lost
+    ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 9); // 1 track lost
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
+    ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
+
+    processor->callReset(); // now incoming is last and last is origin
+
+    // Put a capture on last_ptr_
+    CaptureBasePtr new_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setInc(new_cap);
+
+    auto n_tracked_feat = processor->callProcessKnown();
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), 14); // 14 previously tracked features
+    ASSERT_EQ(n_tracked_feat, 13); // 1 track lost
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 13); // 1 track lost
+    ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 13); // 1 track lost
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, createFactor)
+{
+    FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                     Eigen::Vector1s::Ones(),
+                                                     Eigen::MatrixXs::Ones(1, 1)));
+    FeatureBasePtr ftr_other(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                           Eigen::Vector1s::Ones(),
+                                                           Eigen::MatrixXs::Ones(1, 1)));
+
+    FactorBasePtr fac = processor->callCreateFactor(ftr, ftr_other);
+    fac->link(ftr);
+    ASSERT_EQ(fac->getFeature(),ftr);
+    ASSERT_EQ(fac->getFrameOther(),nullptr);
+    ASSERT_EQ(fac->getCaptureOther(),nullptr);
+    ASSERT_EQ(fac->getFeatureOther(),ftr_other);
+    ASSERT_EQ(fac->getLandmarkOther(),nullptr);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    processor->callProcessNew(15); // detect 15 features, 1 track lost per tracking
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 track lost
+    ASSERT_EQ(processor->getMatchesLastFromIncoming().size(), 14); // 1 track lost
+
+    processor->callReset(); // now incoming is last and last is origin
+
+    // test establishFactors()
+    processor->callEstablishFactors();
+    TrackMatrix track_matrix = processor->getTrackMatrix();
+    unsigned int n_factors_ori = 0;
+    unsigned int n_factors_last = 0;
+    for (auto feat : processor->getLast()->getFeatureList())
+    {
+        if (!feat->getFactorList().empty())
+        {
+            n_factors_last++;
+            ASSERT_EQ(feat->getFactorList().size(), 1);
+            ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+            ASSERT_EQ(feat->getFactorList().front()->getFeatureOther(), track_matrix.feature(feat->trackId(), processor->getOrigin()));
+        }
+    }
+
+    for (auto feat : processor->getOrigin()->getFeatureList())
+    {
+        if (!feat->getConstrainedByList().empty())
+        {
+            n_factors_ori++;
+            ASSERT_EQ(feat->getConstrainedByList().size(), 1);
+            ASSERT_EQ(feat->getConstrainedByList().front()->getFeatureOther(), feat);
+            ASSERT_EQ(feat->getConstrainedByList().front()->getFeature(), track_matrix.feature(feat->trackId(), processor->getLast()));
+        }
+    }
+    ASSERT_EQ(n_factors_ori, 14);
+    ASSERT_EQ(n_factors_last, 14);
+}
+
+TEST_F(ProcessorTrackerFeatureDummyTest, process)
+{
+
+    //1ST TIME -> KF (origin)
+    WOLF_DEBUG("First time...");
+    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());
+
+    //2ND TIME
+    WOLF_DEBUG("Second time...");
+    CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
+    cap2->process();
+
+    ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-1);
+
+    //3RD TIME
+    WOLF_DEBUG("Third time...");
+    CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
+    cap3->process();
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2);
+
+    //4TH TIME
+    WOLF_DEBUG("Forth time...");
+    CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+    cap4->process();
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
+
+    //5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
+    WOLF_DEBUG("Fifth time...");
+    CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
+    cap5->process();
+
+    ASSERT_TRUE(cap4->getFrame()->isKey());
+    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id());
+
+    // check factors
+    WOLF_DEBUG("checking factors...");
+    TrackMatrix track_matrix = processor->getTrackMatrix();
+    ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
+    unsigned int n_factors0 = 0;
+    unsigned int n_factors4 = 0;
+    for (auto feat : cap4->getFeatureList())
+    {
+        if (!feat->getFactorList().empty())
+        {
+            n_factors0++;
+            ASSERT_EQ(feat->getFactorList().size(), 1);
+            ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+            ASSERT_EQ(feat->getFactorList().front()->getFeatureOther(), track_matrix.feature(feat->trackId(), cap1));
+        }
+    }
+
+    for (auto feat : cap1->getFeatureList())
+    {
+        if (!feat->getConstrainedByList().empty())
+        {
+            n_factors4++;
+            ASSERT_EQ(feat->getConstrainedByList().size(), 1);
+            ASSERT_EQ(feat->getConstrainedByList().front()->getFeatureOther(), feat);
+            ASSERT_EQ(feat->getConstrainedByList().front()->getFeature(), track_matrix.feature(feat->trackId(), cap4));
+        }
+    }
+    ASSERT_EQ(n_factors0, 7);
+    ASSERT_EQ(n_factors4, 7);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5f3f5945b13320516d713ab30f535383f273d55
--- /dev/null
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -0,0 +1,349 @@
+
+// wolf includes
+#include "core/utils/utils_gtest.h"
+#include "core/sensor/sensor_factory.h"
+#include "core/processor/processor_tracker_landmark_dummy.h"
+#include "core/capture/capture_void.h"
+
+using namespace wolf;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummyDummy);
+
+class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
+{
+	public:
+
+        ProcessorTrackerLandmarkDummyDummy(ProcessorParamsTrackerLandmarkDummyPtr& _params) :
+            ProcessorTrackerLandmarkDummy(_params){}
+
+		void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; }
+		void setInc(CaptureBasePtr _incoming_ptr){ incoming_ptr_ = _incoming_ptr; }
+
+		unsigned int callProcessKnown(){ return this->processKnown(); }
+
+		unsigned int callProcessNew(const int& _max_new_features){ return this->processNew(_max_new_features); }
+
+        unsigned int callDetectNewFeatures(const int& _max_features,
+                                       FeatureBasePtrList& _features_incoming_out){ return this->detectNewFeatures(_max_features,
+                                                                                                                   _features_incoming_out); }
+
+        unsigned int callFindLandmarks(const LandmarkBasePtrList& _landmarks_in,
+                                       FeatureBasePtrList& _features_incoming_out,
+                                       LandmarkMatchMap& _feature_landmark_correspondences){ return this->findLandmarks(_landmarks_in,
+                                                                                                                        _features_incoming_out,
+                                                                                                                        _feature_landmark_correspondences); }
+
+        LandmarkBasePtr callCreateLandmark(FeatureBasePtr _feature_ptr){ return this->createLandmark(_feature_ptr); }
+        void callCreateNewLandmarks(){ this->createNewLandmarks(); }
+        FactorBasePtr callCreateFactor(FeatureBasePtr _feature_ptr,
+                                       LandmarkBasePtr _landmark_ptr){ return this->createFactor(_feature_ptr,
+                                                                                                 _landmark_ptr); }
+
+        void callEstablishFactors(){ this->establishFactors();}
+
+        void setNewFeaturesLast(FeatureBasePtrList& _new_features_list){ new_features_last_ = _new_features_list;}
+
+        LandmarkMatchMap getMatchesLandmarkFromIncoming() { return matches_landmark_from_incoming_;}
+        LandmarkMatchMap getMatchesLandmarkFromLast() { return matches_landmark_from_last_;}
+        LandmarkBasePtrList getNewLandmarks() { return new_landmarks_;}
+
+        void callReset()
+        {
+            this->resetDerived();
+            origin_ptr_ = last_ptr_;
+            last_ptr_   = incoming_ptr_;
+            incoming_ptr_ = nullptr;
+        };
+};
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class ProcessorTrackerLandmarkDummyTest : public testing::Test
+{
+    public:
+        ProblemPtr problem;
+        SensorBasePtr sensor;
+        ProcessorParamsTrackerLandmarkDummyPtr params;
+        ProcessorTrackerLandmarkDummyDummyPtr processor;
+
+        virtual void SetUp()
+        {
+            // Wolf problem
+            problem = Problem::create("PO", 2);
+
+            // Install camera
+            sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
+
+            // Install processor
+            params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>();
+            params->max_new_features = 10;
+            params->min_features_for_keyframe = 7;
+            params->time_tolerance = 0.25;
+            params->voting_active = true;
+            params->n_landmarks_lost = 1; // 1 (the last) landmark is not found each time findLandmark is called
+            processor  = std::make_shared<ProcessorTrackerLandmarkDummyDummy>(params);
+            processor->link(sensor);
+        }
+};
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, installProcessor)
+{
+    ASSERT_EQ(processor->getProblem(), problem);
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // demo callDetectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, createLandmark)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // demo callDetectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+
+    for (auto ftr : feat_list)
+    {
+        auto lmk = processor->callCreateLandmark(ftr);
+        lmk->link(problem->getMap());
+    }
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // created 10 landmarks
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, createNewLandmarks)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // test detectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+
+    // test createNewLandmarks
+    processor->setNewFeaturesLast(feat_list);
+    processor->callCreateNewLandmarks();
+    ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // created 10 landmarks
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // test detectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
+    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+
+    // test createNewLandmarks
+    processor->setNewFeaturesLast(feat_list);
+    processor->callCreateNewLandmarks();
+    LandmarkBasePtrList new_landmarks = processor->getNewLandmarks();
+    ASSERT_EQ(new_landmarks.size(),n_feat); // created 10 landmarks
+
+    //test findLandmarks
+    LandmarkMatchMap feature_landmark_correspondences;
+    FeatureBasePtrList feat_found;
+    processor->callFindLandmarks(new_landmarks, feat_found, feature_landmark_correspondences);
+    ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size());
+    ASSERT_EQ(feat_list.size(), feat_found.size()+1); // one of each 10 tracks is lost
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    auto n_new_feat = processor->callProcessNew(10); // detect 10 features
+
+    ASSERT_EQ(n_new_feat, 10); // detected 10 features
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), 10); // detected 10 features
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 10); // created 10 landmarks
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found
+    ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 10); // all last features have the landmark correspondence
+    ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
+{
+    // create 10 landmarks and link them to map
+    FeatureBasePtrList feat_list;
+    processor->callDetectNewFeatures(params->max_new_features, feat_list);
+    for (auto ftr : feat_list)
+    {
+        auto lmk = processor->callCreateLandmark(ftr);
+        lmk->link(problem->getMap());
+    }
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(),10); // created 10 landmarks
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    // Test processKnown
+    processor->callProcessKnown();
+
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 9); // 1 of each 10 landmarks is not found
+    ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 9); // 1 of each 10 landmarks is not found
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, createFactor)
+{
+    FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
+                                                     Eigen::Vector1s::Ones(),
+                                                     Eigen::MatrixXs::Ones(1, 1)));
+    LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("BASE",
+                                                       std::make_shared<StateBlock>(1),
+                                                       std::make_shared<StateBlock>(1)));
+
+    FactorBasePtr fac = processor->callCreateFactor(ftr, lmk);
+    fac->link(ftr);
+    ASSERT_EQ(fac->getFeature(),ftr);
+    ASSERT_EQ(fac->getFrameOther(),nullptr);
+    ASSERT_EQ(fac->getCaptureOther(),nullptr);
+    ASSERT_EQ(fac->getFeatureOther(),nullptr);
+    ASSERT_EQ(fac->getLandmarkOther(),lmk);
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    // Put a capture on incoming_ptr_
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(1, sensor);
+    processor->setInc(inc_cap);
+
+    processor->callProcessNew(15); // detect 15 features, 1 of each 10 tracks is lost
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), 15); // detected 15 features
+    ASSERT_EQ(processor->getIncoming()->getFeatureList().size(), 14); // 1 of each 10 tracks is lost
+    ASSERT_EQ(processor->getMatchesLandmarkFromLast().size(), 15); // all landmarks
+    ASSERT_EQ(processor->getMatchesLandmarkFromIncoming().size(), 14); // 1 of each 10 tracks is lost
+
+    // test establishFactors()
+    processor->callEstablishFactors();
+    LandmarkMatchMap landmark_from_last = processor->getMatchesLandmarkFromLast();
+    unsigned int n_factors_last = 0;
+    unsigned int n_factors_landmark = 0;
+    for (auto feat : processor->getLast()->getFeatureList())
+    {
+        n_factors_last++;
+        ASSERT_EQ(feat->getFactorList().size(), 1);
+        ASSERT_EQ(feat->getFactorList().front()->getFeature(), feat);
+        ASSERT_EQ(feat->getFactorList().front()->getLandmarkOther(), landmark_from_last[feat]->landmark_ptr_);
+    }
+
+    for (auto lmk : problem->getMap()->getLandmarkList())
+    {
+        n_factors_landmark++;
+        ASSERT_EQ(lmk->getConstrainedByList().size(), 1);
+        ASSERT_EQ(lmk->getConstrainedByList().front()->getLandmarkOther(), lmk);
+        ASSERT_EQ(landmark_from_last[lmk->getConstrainedByList().front()->getFeature()]->landmark_ptr_, lmk);
+    }
+    ASSERT_EQ(n_factors_last, 15);
+    ASSERT_EQ(n_factors_landmark, 15);
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, process)
+{
+
+    //1ST TIME -> KF (origin)
+    WOLF_DEBUG("First time...");
+    CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
+    cap1->process();
+
+    ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
+    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame());
+
+    //2ND TIME
+    WOLF_DEBUG("Second time...");
+    CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
+    cap2->process();
+
+    ASSERT_EQ(processor->getOrigin()->getFeatureList().size(), params->max_new_features);
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-1);
+
+    //3RD TIME
+    WOLF_DEBUG("Third time...");
+    CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
+    cap3->process();
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-2);
+
+    //4TH TIME
+    WOLF_DEBUG("Forth time...");
+    CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+    cap4->process();
+
+    ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);
+
+    //5TH TIME -> KF in cap4 (found landmarks < 7 (params->min_features_for_keyframe))
+    WOLF_DEBUG("Fifth time...");
+    CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
+    cap5->process();
+
+    ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame());
+    ASSERT_EQ(processor->getOrigin(), cap4);
+    ASSERT_EQ(processor->getLast(), cap5);
+
+    // check factors
+    WOLF_DEBUG("checking factors...");
+    LandmarkMatchMap landmark_from_last = processor->getMatchesLandmarkFromLast();
+    unsigned int n_factors_cap1 = 0;
+    unsigned int n_factors_cap4 = 0;
+
+    for (auto lmk : problem->getMap()->getLandmarkList())
+    {
+        for (auto fac : lmk->getConstrainedByList())
+        {
+            ASSERT_EQ(fac->getFeature()->getFactorList().size(), 1); // only one factor per feature
+            if (fac->getFeature()->getCapture() == cap1)
+                n_factors_cap1++;
+            else if (fac->getFeature()->getCapture() == cap4)
+                n_factors_cap4++;
+            else
+                ASSERT_TRUE(false);// this shouldn't happen!
+        }
+    }
+    ASSERT_EQ(n_factors_cap1, 10);
+    ASSERT_EQ(n_factors_cap4, 17);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index ff930b512b8f8da960b3921f81bc2bd73eac03b5..f2f08e8c2241624b8914fd0eb6dc2ebbf93bab2d 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -19,11 +19,18 @@ class TrackMatrixTest : public testing::Test
         Eigen::Vector2s m;
         Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01;
 
+        FrameBasePtr   F0, F1, F2, F3, F4;
         CaptureBasePtr C0, C1, C2, C3, C4;
         FeatureBasePtr f0, f1, f2, f3, f4;
 
         virtual void SetUp()
         {
+            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);
+
             C0 = std::make_shared<CaptureBase>("BASE", 0.0);
             C1 = std::make_shared<CaptureBase>("BASE", 1.0);
             C2 = std::make_shared<CaptureBase>("BASE", 2.0);
@@ -35,6 +42,17 @@ class TrackMatrixTest : public testing::Test
             f2 = std::make_shared<FeatureBase>("BASE", m, m_cov);
             f3 = std::make_shared<FeatureBase>("BASE", m, m_cov);
             f4 = std::make_shared<FeatureBase>("BASE", m, m_cov);
+
+            // F0 and F4 are keyframes
+            F0->setKey();
+            F4->setKey();
+
+            // link captures
+            C0->link(F0);
+            C1->link(F1);
+            C2->link(F2);
+            C3->link(F3);
+            C4->link(F4);
         }
 };
 
@@ -45,27 +63,47 @@ TEST_F(TrackMatrixTest, newTrack)
 
     track_matrix.newTrack(C0, f1);
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+
+    track_matrix.newTrack(C1, f2);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3);
+
+    track_matrix.newTrack(C1, f3);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4);
 }
 
 TEST_F(TrackMatrixTest, add)
 {
     track_matrix.newTrack(C0, f0);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
 
     track_matrix.add(f0->trackId(), C1, f1);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1   C2   snapshots
      *
      *  f0---f1        trk 0
      */
     ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
     ASSERT_EQ(f1->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
 
     track_matrix.add(f0->trackId(), C2, f2);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1   C2   snapshots
      *
      *  f0---f1---f2   trk 0
      */
     ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
     ASSERT_EQ(f2->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+
+    track_matrix.add(f0->trackId(), C2, f2);
+    /* KC0   C1   C2   snapshots
+     *
+     *  f0---f1---f2   trk 0
+     *       f3        trk 1
+     */
+    track_matrix.add(1, C1, f3);
+    ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1);
+    ASSERT_NE(f3->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
 }
 
 TEST_F(TrackMatrixTest, numTracks)
@@ -153,8 +191,18 @@ TEST_F(TrackMatrixTest, remove_ftr)
     ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1);
 
     track_matrix.remove(f1);
+    /*  C0   C1   C2   snapshots
+     *
+     *  f2             trk 1
+     */
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
     ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
+
+    track_matrix.remove(f2);
+    /*  C0   C1   C2   snapshots
+     *
+     */
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
 }
 
 TEST_F(TrackMatrixTest, remove_trk)
@@ -321,6 +369,30 @@ TEST_F(TrackMatrixTest, matches)
     ASSERT_EQ(pairs.size(), (unsigned int) 0);
 }
 
+TEST_F(TrackMatrixTest, trackAtKeyframes)
+{
+    track_matrix.newTrack(C0, f0);
+    track_matrix.add(f0->trackId(), C1, f1);
+    track_matrix.add(f0->trackId(), C2, f2);
+    track_matrix.add(f0->trackId(), C4, f4);
+    track_matrix.newTrack(C1, f3);
+    /* KC0   C1   C2   C3  KC4 snapshots
+     *
+     *  f0---f1---f2--------f4 trk 0
+     *       |
+     *       f3                trk 1
+     */
+
+    wolf::Track trk_kf_0 = track_matrix.trackAtKeyframes(f0->trackId());
+    ASSERT_EQ(trk_kf_0.size(), 2);
+    ASSERT_EQ(trk_kf_0[0.0], f0);
+    ASSERT_EQ(trk_kf_0[4.0], f4);
+
+    wolf::Track trk_kf_1 = track_matrix.trackAtKeyframes(f3->trackId());
+    ASSERT_EQ(trk_kf_1.size(), 0);
+}
+
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
diff --git a/demos/demo_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
similarity index 55%
rename from demos/demo_yaml_conversions.cpp
rename to test/gtest_yaml_conversions.cpp
index b8e1f41f9c0f8b6ae50e65c4fec943dbb1305a7d..3fce436c95600377d547d01a375a2c204c059588 100644
--- a/demos/demo_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -5,19 +5,23 @@
  *      \author: jsola
  */
 
+#include "core/common/wolf.h"
+#include "core/utils/utils_gtest.h"
 #include "core/yaml/yaml_conversion.h"
-
 #include <yaml-cpp/yaml.h>
-
 #include <eigen3/Eigen/Dense>
-
 #include <iostream>
 //#include <fstream>
 
-int main()
-{
+using namespace Eigen;
+using namespace wolf;
 
-    using namespace Eigen;
+TEST(MapYaml, save_2D)
+{
+    MatrixXs M23(2,3);
+    MatrixXs M33(3,3);
+    M23 << 1, 2, 3, 4, 5, 6;
+    M33 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
 
     std::cout << "\nTrying different yaml specs for matrix..." << std::endl;
 
@@ -29,26 +33,37 @@ int main()
     mat_23      = YAML::Load("[1, 2, 3, 4, 5, 6]"); // insensitive to spacing
     mat_33      = YAML::Load("[1, 2, 3, 4, 5, 6, 7, 8, 9]"); // insensitive to spacing
 
-    MatrixXd Mx = mat_sized_23.as<MatrixXd>();
+    MatrixXs Mx = mat_sized_23.as<MatrixXs>();
     std::cout << "Dyn-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << Mx << std::endl;
+    ASSERT_MATRIX_APPROX(Mx, M23, 1e-12);
 
-    Matrix<double, 2, Dynamic> M2D = mat_sized_23.as<Matrix<double, 2, Dynamic>>();
+    Matrix<Scalar, 2, Dynamic> M2D = mat_sized_23.as<Matrix<Scalar, 2, Dynamic>>();
     std::cout << "2-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << M2D << std::endl;
+    ASSERT_MATRIX_APPROX(M2D, M23, 1e-12);
 
-    Matrix<double, Dynamic, 3> MD3 = mat_sized_23.as<Matrix<double, Dynamic, 3>>();
+    Matrix<Scalar, Dynamic, 3> MD3 = mat_sized_23.as<Matrix<Scalar, Dynamic, 3>>();
     std::cout << "Dyn-3 [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << MD3 << std::endl;
+    ASSERT_MATRIX_APPROX(MD3, M23, 1e-12);
 
-    Matrix3d M3 = mat_sized_33.as<Matrix3d>();
+    Matrix3s M3 = mat_sized_33.as<Matrix3s>();
     std::cout << "3-3   [[3,3] ,[1, 2, 3, 4, 5, 6, 7, 8, 9] ] = \n" << M3 << std::endl;
+    ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
 
-    M2D = mat_23.as<Matrix<double, 2, Dynamic>>();
+    M2D = mat_23.as<Matrix<Scalar, 2, Dynamic>>();
     std::cout << "2-Dyn [1, 2, 3, 4, 5, 6] = \n" << M2D << std::endl;
+    ASSERT_MATRIX_APPROX(M2D, M23, 1e-12);
 
-    MD3 = mat_23.as<Matrix<double, Dynamic, 3>>();
+    MD3 = mat_23.as<Matrix<Scalar, Dynamic, 3>>();
     std::cout << "Dyn-3 [1, 2, 3, 4, 5, 6] = \n" << MD3 << std::endl;
+    ASSERT_MATRIX_APPROX(MD3, M23, 1e-12);
 
-    M3 = mat_33.as<Matrix3d>();
+    M3 = mat_33.as<Matrix3s>();
     std::cout << "3-3   [1, 2, 3, 4, 5, 6, 7, 8, 9] = \n" << M3 << std::endl;
+    ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
+}
 
-    return 0;
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
 }
diff --git a/test/params1.yaml b/test/params1.yaml
deleted file mode 100644
index 4ad74198329b61d86b023f17f9a7016224d5489a..0000000000000000000000000000000000000000
--- a/test/params1.yaml
+++ /dev/null
@@ -1 +0,0 @@
-      follow: "/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/params3.yaml b/test/params3.yaml
deleted file mode 100644
index 78489d218fad394f90364a1e1d758daa0270e7cd..0000000000000000000000000000000000000000
--- a/test/params3.yaml
+++ /dev/null
@@ -1 +0,0 @@
-      jump: "@/test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index 940d3e5854a5ec612f5223251243e643c7bef79b..d7d066b53a333d6beb27b20d7eef7468b29f505e 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -1,4 +1,7 @@
 config:
+  problem:
+    frame structure: "POV"
+    dimension: 3
   sensors: 
     -
       type: "ODOM 2D"
@@ -24,7 +27,7 @@ config:
       type: "ODOM 2D"
       name: "my_proc_test"
       sensor name: "odom"
-      follow: "/test/yaml/params3.1.yaml"
+      follow: "test/yaml/params3.1.yaml"
 files:
   - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
   - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
index d58014fbabc36387be1a96cc4244cff954ac2820..7830e8cb85f9fef0acea46e81fa293ed7528fc5c 100644
--- a/test/yaml/params2.yaml
+++ b/test/yaml/params2.yaml
@@ -1,4 +1,7 @@
 config:
+  problem:
+    frame structure: "POV"
+    dimension: 2
   sensors: 
     -
       type: "ODOM 2D"
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index ac82cf0dd818e6c6dec00c61ff86b75c38a3fc11..3e0d7a40f9fce04d9f6976518fe3bb900dabdbf9 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -1,31 +1,19 @@
 config:
-  sensors: 
+  # problem:
+  #   frame structure: "POV"
+  #   dimension: 2
+  sensors:
     -
       type: "ODOM 2D"
       name: "odom"
       intrinsic:
         k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
+        k_rot_to_rot: 0.1
       extrinsic:
         pos: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
   processors:
-    -
-      type: "ODOM 2D"
-      name: "processor1"
-      sensor name: "odom"
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor name: "rb"
     -
       type: "ODOM 2D"
       name: "my_proc_test"
       sensor name: "odom"
-      follow: "/test/yaml/params3.1.yaml"
-      jump: "@/test/yaml/params3.1.yaml"
-files:
-  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
-  - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
+      extern params: "@test/yaml/params3.1.yaml"
\ No newline at end of file