diff --git a/CMakeLists.txt b/CMakeLists.txt
index a5b1e21c2f3510bd97ad176c181cfa3d6d171264..f688e52678b509edbbd9b6299da740fd86981ba6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -100,7 +100,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 # option(BUILD_EXAMPLES "Build examples" OFF)
 set(BUILD_TESTS true)
-set(BUILD_EXAMPLES true)
+set(BUILD_EXAMPLES false)
 
 # Does this has any other interest
 # but for the examples ?
@@ -254,42 +254,54 @@ ENDIF(GLOG_FOUND)
 
 #HEADERS
 
-SET(HDRS_BASE
-  include/base/IMU_tools.h
-  include/base/SE3.h
-  include/base/converter.h
-  include/base/diff_drive_tools.h
-  include/base/diff_drive_tools.hpp
-  include/base/eigen_assert.h
-  include/base/eigen_predicates.h
-  include/base/factory.h
-  include/base/frame_base.h
-  include/base/hardware_base.h
-  include/base/loader.hpp
-  include/base/local_parametrization_angle.h
-  include/base/local_parametrization_base.h
-  include/base/local_parametrization_homogeneous.h
-  include/base/local_parametrization_polyline_extreme.h
-  include/base/local_parametrization_quaternion.h
-  include/base/logging.h
-  include/base/make_unique.h
-  include/base/map_base.h
-  include/base/motion_buffer.h
-  include/base/node_base.h
-  include/base/params_server.hpp
-  include/base/pinhole_tools.h
-  include/base/problem.h
-  include/base/rotations.h
-  include/base/singleton.h
-  include/base/state_angle.h
-  include/base/state_block.h
-  include/base/state_homogeneous_3D.h
-  include/base/state_quaternion.h
-  include/base/time_stamp.h
-  include/base/track_matrix.h
-  include/base/trajectory_base.h
-  include/base/wolf.h
+
+SET(HDRS_COMMON
+  include/base/common/factory.h
+  include/base/common/node_base.h
+  include/base/common/time_stamp.h
+  include/base/common/wolf.h
+  )
+SET(HDRS_MATH
+  include/base/math/pinhole_tools.h
+  include/base/math/SE3.h
+  include/base/math/IMU_tools.h
+  include/base/math/rotations.h
+  )
+SET(HDRS_UTILS
+  include/base/utils/eigen_assert.h
+  include/base/utils/eigen_predicates.h
+  include/base/utils/logging.h
+  include/base/utils/singleton.h
+  include/base/utils/make_unique.h
+  )
+SET(HDRS_PROBLEM
+  include/base/problem/problem.h
+  )
+SET(HDRS_HARDWARE
+  include/base/hardware/hardware_base.h
   )
+SET(HDRS_TRAJECTORY
+  include/base/trajectory/trajectory_base.h
+  )
+SET(HDRS_MAP
+  include/base/map/map_base.h
+  )
+SET(HDRS_FRAME
+  include/base/frame/frame_base.h
+  )
+SET(HDRS_STATE_BLOCK
+  include/base/state_block/local_parametrization_angle.h
+  include/base/state_block/local_parametrization_base.h
+  include/base/state_block/local_parametrization_homogeneous.h
+  include/base/state_block/local_parametrization_polyline_extreme.h
+  include/base/state_block/local_parametrization_quaternion.h
+  include/base/state_block/state_angle.h
+  include/base/state_block/state_block.h
+  include/base/state_block/state_homogeneous_3D.h
+  include/base/state_block/state_quaternion.h
+  include/base/state_block/local_parametrization_polyline_extreme.h
+  )
+
 SET(HDRS_CAPTURE
   include/base/capture/capture_GPS_fix.h
   include/base/capture/capture_IMU.h
@@ -371,6 +383,10 @@ SET(HDRS_LANDMARK
   )
 SET(HDRS_PROCESSOR
   include/base/processor/new_processor_factory.h
+  include/base/processor/diff_drive_tools.h
+  include/base/processor/diff_drive_tools.hpp
+  include/base/processor/motion_buffer.h
+  include/base/processor/processor_IMU.h
   include/base/processor/processor_IMU.h
   include/base/processor/processor_base.h
   include/base/processor/processor_capture_holder.h
@@ -387,6 +403,8 @@ SET(HDRS_PROCESSOR
   include/base/processor/processor_tracker_feature_dummy.h
   include/base/processor/processor_tracker_landmark.h
   include/base/processor/processor_tracker_landmark_dummy.h
+  include/base/processor/processor_tracker_landmark_dummy.h
+  include/base/processor/track_matrix.h
   )
 SET(HDRS_SENSOR
   include/base/sensor/new_sensor_factory.h
@@ -403,10 +421,8 @@ SET(HDRS_SENSOR
 SET(HDRS_SOLVER
   include/base/solver/solver_manager.h
   )
-# [Add generic derived header before this line]
 
 SET(HDRS_DTASSC
-  include/base/track_matrix.h
   include/base/association/association_solver.h
   include/base/association/association_node.h
   include/base/association/association_tree.h
@@ -414,92 +430,50 @@ SET(HDRS_DTASSC
   include/base/association/matrix.h
   )
 
-SET(HDRS_CORE
-  include/base/SE3.h
-  include/base/capture/capture_base.h
-  include/base/capture/capture_buffer.h
-  include/base/capture/capture_pose.h
-  include/base/capture/capture_void.h
-  include/base/factor/factor_analytic.h
-  include/base/factor/factor_autodiff.h
-  include/base/factor/factor_base.h
-  include/base/processor/processor_factory.h
-  include/base/feature/feature_base.h
-  include/base/feature/feature_match.h
-  include/base/feature/feature_pose.h
-  include/base/frame_base.h
-  include/base/hardware_base.h
-  include/base/landmark/landmark_base.h
-  include/base/local_parametrization_angle.h
-  include/base/local_parametrization_base.h
-  include/base/local_parametrization_homogeneous.h
-  include/base/local_parametrization_quaternion.h
-  include/base/map_base.h
-  include/base/motion_buffer.h
-  include/base/node_base.h
-  include/base/problem.h
-  include/base/processor/processor_base.h
-  include/base/processor/processor_factory.h
-  include/base/processor/processor_logging.h
-  include/base/processor/processor_loopclosure_base.h
-  include/base/processor/processor_motion.h
-  include/base/processor/processor_tracker.h
-  include/base/processor/processor_tracker_feature.h
-  include/base/rotations.h
-  include/base/sensor/sensor_base.h
-  include/base/singleton.h
-  include/base/state_angle.h
-  include/base/state_block.h
-  include/base/state_homogeneous_3D.h
-  include/base/state_quaternion.h
-  include/base/time_stamp.h
-  include/base/track_matrix.h
-  include/base/trajectory_base.h
-  include/base/wolf.h
+
+#SOURCES
+SET(SRCS_PROBLEM
+  src/problem/problem.cpp
   )
 SET(HDRS_YAML
   include/base/yaml/parser_yaml.hpp
   )
-#SOURCES
-SET(SRCS_CORE
-  src/capture/capture_base.cpp
-  src/capture/capture_pose.cpp
-  src/capture/capture_void.cpp
-  src/factor/factor_analytic.cpp
-  src/factor/factor_base.cpp
-  src/feature/feature_base.cpp
-  src/feature/feature_pose.cpp
-  src/frame_base.cpp
-  src/hardware_base.cpp
-  src/landmark/landmark_base.cpp
-  src/local_parametrization_base.cpp
-  src/local_parametrization_homogeneous.cpp
-  src/local_parametrization_quaternion.cpp
-  src/map_base.cpp
-  src/motion_buffer.cpp
-  src/node_base.cpp
-  src/problem.cpp
-  src/processor/processor_base.cpp
-  src/processor/processor_loopclosure_base.cpp
-  src/processor/processor_motion.cpp
-  src/processor/processor_tracker.cpp
-  src/sensor/sensor_base.cpp
-  src/state_block.cpp
-  src/time_stamp.cpp
-  src/track_matrix.cpp
-  src/trajectory_base.cpp
+SET(SRCS_HARDWARE
+  src/hardware/hardware_base.cpp
   )
-
-SET(SRCS_BASE
-  src/capture/capture_motion.cpp
-  src/processor/processor_capture_holder.cpp
+SET(SRCS_TRAJECTORY
+  src/trajectory/trajectory_base.cpp
+  )
+SET(SRCS_MAP
+  src/map/map_base.cpp
+  )
+SET(SRCS_FRAME
+  src/frame/frame_base.cpp
+  )
+SET(SRCS_STATE_BLOCK
+  src/state_block/state_block.cpp
+  src/state_block/local_parametrization_base.cpp
+  src/state_block/local_parametrization_homogeneous.cpp
+  src/state_block/local_parametrization_quaternion.cpp
+  src/state_block/local_parametrization_polyline_extreme.cpp
+  )
+SET(SRCS_COMMON
+  src/common/node_base.cpp
+  src/common/time_stamp.cpp
+  )
+SET(SRCS_MATH
+  )
+SET(SRCS_UTILS
   )
 
 SET(SRCS
-  src/local_parametrization_polyline_extreme.cpp
   test/processor_IMU_UnitTester.cpp
   )
 SET(SRCS_CAPTURE
+  src/capture/capture_motion.cpp
+  src/capture/capture_base.cpp
+  src/capture/capture_pose.cpp
+  src/capture/capture_void.cpp
   src/capture/capture_GPS_fix.cpp
   src/capture/capture_IMU.cpp
   src/capture/capture_odom_2D.cpp
@@ -507,33 +481,50 @@ SET(SRCS_CAPTURE
   src/capture/capture_velocity.cpp
   src/capture/capture_wheel_joint_position.cpp
   )
+SET(SRCS_FACTOR
+  src/factor/factor_analytic.cpp
+  src/factor/factor_base.cpp
+  )
 SET(SRCS_FEATURE
   src/feature/feature_GPS_fix.cpp
   src/feature/feature_GPS_pseudorange.cpp
   src/feature/feature_IMU.cpp
+  src/feature/feature_base.cpp
   src/feature/feature_corner_2D.cpp
   src/feature/feature_diff_drive.cpp
   src/feature/feature_odom_2D.cpp
   src/feature/feature_polyline_2D.cpp
+  src/feature/feature_pose.cpp
   )
 SET(SRCS_LANDMARK
   src/landmark/landmark_corner_2D.cpp
   src/landmark/landmark_container.cpp
   src/landmark/landmark_line_2D.cpp
   src/landmark/landmark_polyline_2D.cpp
+  src/landmark/landmark_base.cpp
   )
 SET(SRCS_PROCESSOR
+  src/processor/motion_buffer.cpp
   src/processor/processor_IMU.cpp
+  src/processor/processor_base.cpp
+  src/processor/processor_capture_holder.cpp
   src/processor/processor_diff_drive.cpp
   src/processor/processor_frame_nearest_neighbor_filter.cpp
+  src/processor/processor_loopclosure_base.cpp
+  src/processor/processor_motion.cpp
   src/processor/processor_odom_2D.cpp
   src/processor/processor_odom_3D.cpp
+  src/processor/processor_tracker.cpp
   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/track_matrix.cpp
   )
 SET(SRCS_SENSOR
+  src/sensor/sensor_base.cpp
+  src/sensor/sensor_camera.cpp
+  src/sensor/sensor_diff_drive.cpp
   src/sensor/sensor_GPS.cpp
   src/sensor/sensor_GPS_fix.cpp
   src/sensor/sensor_IMU.cpp
@@ -651,15 +642,12 @@ ENDIF(Suitesparse_FOUND)
 # LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
 IF(YAMLCPP_FOUND)
   # headers
-  SET(HDRS ${HDRS}
-    include/base/yaml/yaml_conversion.h
-    )
   SET(HDRS_YAML ${HDRS_YAML}
     include/base/yaml/yaml_conversion.h
     )
 
   # sources
-  SET(SRCS ${SRCS}
+  SET(SRCS_YAML ${SRCS_YAML}
     src/yaml/processor_odom_3D_yaml.cpp
     src/yaml/processor_IMU_yaml.cpp
     src/yaml/sensor_camera_yaml.cpp
@@ -667,12 +655,12 @@ IF(YAMLCPP_FOUND)
     src/yaml/sensor_IMU_yaml.cpp
     )
   IF(laser_scan_utils_FOUND)
-    SET(SRCS ${SRCS}
+    SET(SRCS ${SRCS_YAML}
       src/yaml/sensor_laser_2D_yaml.cpp
       )
   ENDIF(laser_scan_utils_FOUND)
   IF(vision_utils_FOUND)
-    SET(SRCS ${SRCS}
+    SET(SRCS_YAML ${SRCS_YAML}
       src/yaml/processor_image_yaml.cpp
       src/yaml/processor_tracker_feature_trifocal_yaml.cpp
       )
@@ -680,20 +668,28 @@ IF(YAMLCPP_FOUND)
 ENDIF(YAMLCPP_FOUND)
 
 # create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
-  SHARED
-  ${SRCS_BASE}
-  ${SRCS_CORE}
-  ${SRCS}
+ADD_LIBRARY(${PROJECT_NAME} 
+  SHARED 
+  ${SRCS_BASE} 
   ${SRCS_CAPTURE}
+  ${SRCS_COMMON}
+  ${SRCS_DTASSC} 
   ${SRCS_FACTOR}
   ${SRCS_FEATURE}
+  ${SRCS_FRAME}
+  ${SRCS_HARDWARE}
   ${SRCS_LANDMARK}
+  ${SRCS_MAP}
+  ${SRCS_MATH}
+  ${SRCS_PROBLEM}
   ${SRCS_PROCESSOR}
   ${SRCS_SENSOR}
-  #${SRCS_DTASSC}
   ${SRCS_SOLVER}
+  ${SRCS_STATE_BLOCK}
+  ${SRCS_TRAJECTORY}
+  ${SRCS_UTILS}
   ${SRCS_WRAPPER}
+  ${SRCS_YAML}
   )
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 
@@ -743,40 +739,56 @@ ENDIF(BUILD_EXAMPLES)
 
 #=============================================================
 INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
-      RUNTIME DESTINATION bin
-      LIBRARY DESTINATION lib/iri-algorithms
-      ARCHIVE DESTINATION lib/iri-algorithms)
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib/iri-algorithms
+  ARCHIVE DESTINATION lib/iri-algorithms)
 
 install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
 #install headers
-INSTALL(FILES ${HDRS_BASE}
-    DESTINATION include/iri-algorithms/wolf/base)
+INSTALL(FILES ${HDRS_MATH}
+  DESTINATION include/iri-algorithms/wolf/base/math)
+INSTALL(FILES ${HDRS_UTILS}
+  DESTINATION include/iri-algorithms/wolf/base/utils)
+INSTALL(FILES ${HDRS_PROBLEM}
+  DESTINATION include/iri-algorithms/wolf/base/problem)
+INSTALL(FILES ${HDRS_HARDWARE}
+  DESTINATION include/iri-algorithms/wolf/base/hardware)
+INSTALL(FILES ${HDRS_TRAJECTORY}
+  DESTINATION include/iri-algorithms/wolf/base/trajectory)
+INSTALL(FILES ${HDRS_MAP}
+  DESTINATION include/iri-algorithms/wolf/base/map)
+INSTALL(FILES ${HDRS_FRAME}
+  DESTINATION include/iri-algorithms/wolf/base/frame)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+  DESTINATION include/iri-algorithms/wolf/base/state_block)
+INSTALL(FILES ${HDRS_COMMON}
+  DESTINATION include/iri-algorithms/wolf/base/common)
 INSTALL(FILES ${HDRS_DTASSC}
-   DESTINATION include/iri-algorithms/wolf/base/association)
+  DESTINATION include/iri-algorithms/wolf/base/association)
 INSTALL(FILES ${HDRS_CAPTURE}
-    DESTINATION include/iri-algorithms/wolf/base/capture)
+   DESTINATION include/iri-algorithms/wolf/base/capture)
 INSTALL(FILES ${HDRS_FACTOR}
-    DESTINATION include/iri-algorithms/wolf/base/factor)
+  DESTINATION include/iri-algorithms/wolf/base/factor)
 INSTALL(FILES ${HDRS_FEATURE}
-    DESTINATION include/iri-algorithms/wolf/base/feature)
+  DESTINATION include/iri-algorithms/wolf/base/feature)
 INSTALL(FILES ${HDRS_SENSOR}
-    DESTINATION include/iri-algorithms/wolf/base/sensor)
+  DESTINATION include/iri-algorithms/wolf/base/sensor)
 INSTALL(FILES ${HDRS_PROCESSOR}
-    DESTINATION include/iri-algorithms/wolf/base/processor)
+  DESTINATION include/iri-algorithms/wolf/base/processor)
 INSTALL(FILES ${HDRS_LANDMARK}
-    DESTINATION include/iri-algorithms/wolf/base/landmark)
+  DESTINATION include/iri-algorithms/wolf/base/landmark)
 INSTALL(FILES ${HDRS_WRAPPER}
-    DESTINATION include/iri-algorithms/wolf/base/ceres_wrapper)
+  DESTINATION include/iri-algorithms/wolf/base/ceres_wrapper)
 INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-   DESTINATION include/iri-algorithms/wolf/base/solver_suitesparse)
+  DESTINATION include/iri-algorithms/wolf/base/solver_suitesparse)
 INSTALL(FILES ${HDRS_SOLVER}
-    DESTINATION include/iri-algorithms/wolf/base/solver)
+  DESTINATION include/iri-algorithms/wolf/base/solver)
 INSTALL(FILES ${HDRS_SERIALIZATION}
-    DESTINATION include/iri-algorithms/wolf/base/serialization)
+  DESTINATION include/iri-algorithms/wolf/base/serialization)
 INSTALL(FILES ${HDRS_YAML}
-    DESTINATION include/iri-algorithms/wolf/base/yaml)
+  DESTINATION include/iri-algorithms/wolf/base/yaml)
 INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
-    DESTINATION "lib/cmake/${PROJECT_NAME}")
+  DESTINATION "lib/cmake/${PROJECT_NAME}")
 
 FILE(WRITE wolf.found "")
 INSTALL(FILES wolf.found
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index f7c8d1c0d158f13cc8065dd8a077b8bc7a0c49bf..3708f51d3aa1141756dc54c27f76039671bfa42e 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -12,7 +12,7 @@
  *      \author: jsola
  */
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 #include "base/sensor/sensor_odom_2D.h"
 #include "base/processor/processor_odom_2D.h"
@@ -246,9 +246,17 @@ int main()
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
     for (auto kf : problem->getTrajectory()->getFrameList())
         if (kf->isKey())
-            WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
+        {
+            Eigen::MatrixXs cov;
+            kf->getCovariance(cov);
+            WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+        }
     for (auto lmk : problem->getMap()->getLandmarkList())
-        WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
+        {
+            Eigen::MatrixXs cov;
+            lmk->getCovariance(cov);
+            WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+        }
     std::cout << std::endl;
 
     WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index 339a2e44ea052c2cd082f4a1c36eae813372ac32..dee24fd62e0b55d453359283681032de9fce4038 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/hello_wolf/sensor_range_bearing.cpp
@@ -6,7 +6,7 @@
  */
 
 #include "sensor_range_bearing.h"
-#include "base/state_angle.h"
+#include "base/state_block/state_angle.h"
 
 namespace wolf
 {
diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h
index 6e2de9d9d91eee27afb6b46940f2c5558ee0fddb..9308c1bc0aa44a56b472fc8c67c74da449776318 100644
--- a/include/base/capture/capture_IMU.h
+++ b/include/base/capture/capture_IMU.h
@@ -2,7 +2,7 @@
 #define CAPTURE_IMU_H
 
 //Wolf includes
-#include "base/IMU_tools.h"
+#include "base/math/IMU_tools.h"
 #include "base/capture/capture_motion.h"
 
 namespace wolf {
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index 0082d9d314fa28cf9f26a64b0cb455ab07102244..b3adeb9be3d09e55c8727e687d75569bc408cd0a 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -8,9 +8,9 @@ class FeatureBase;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
 
 //std includes
 
@@ -110,9 +110,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
 }
 
 #include "base/sensor/sensor_base.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 #include "base/feature/feature_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 namespace wolf{
 
diff --git a/include/base/capture/capture_buffer.h b/include/base/capture/capture_buffer.h
index 434716dcfdf596b09d037a62f136428bc9d3e893..393d7fc2489a5035b168b2684788b835a772ab6e 100644
--- a/include/base/capture/capture_buffer.h
+++ b/include/base/capture/capture_buffer.h
@@ -8,8 +8,8 @@
 #ifndef _WOLF_CAPTURE_BUFFER_H_
 #define _WOLF_CAPTURE_BUFFER_H_
 
-#include "base/wolf.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/time_stamp.h"
 
 #include <list>
 #include <algorithm>
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index ed71171e99ef6d221777f1ab89a7e0ba4a8dc0d7..0f84eb72d1737c8e208d25ceb9cacd224b3627b3 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -10,7 +10,7 @@
 
 // Wolf includes
 #include "base/capture/capture_base.h"
-#include "base/motion_buffer.h"
+#include "base/processor/motion_buffer.h"
 
 // STL includes
 #include <list>
diff --git a/include/base/capture/capture_odom_2D.h b/include/base/capture/capture_odom_2D.h
index eafb6ead7bbe4b12f07234dc896f8fe7729b07bf..ff532803c44cbe2857eae956152742560a34e270 100644
--- a/include/base/capture/capture_odom_2D.h
+++ b/include/base/capture/capture_odom_2D.h
@@ -10,7 +10,7 @@
 
 #include "base/capture/capture_motion.h"
 
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf
 {
diff --git a/include/base/capture/capture_odom_3D.h b/include/base/capture/capture_odom_3D.h
index d1f29508bb60fa7bae5c620b7b010f368e00f100..863add2684a022fc1235b6b532be5ed01655a428 100644
--- a/include/base/capture/capture_odom_3D.h
+++ b/include/base/capture/capture_odom_3D.h
@@ -10,7 +10,7 @@
 
 #include "base/capture/capture_motion.h"
 
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf
 {
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
index 4a3f42dcbbc1aeea006e7843941707d4a2b9e5c5..2847d9b9a675962fee76e95e3673537568ef4a3f 100644
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ b/include/base/ceres_wrapper/cost_function_wrapper.h
@@ -2,7 +2,7 @@
 #define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
 
 // WOLF
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "base/factor/factor_analytic.h"
 
 // CERES
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
index fc046a7ec002c29b702133e89fb950b456a764d1..31a2913ec22bc0e5dc5ba6b2757e49508d195578 100644
--- a/include/base/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/base/ceres_wrapper/local_parametrization_wrapper.h
@@ -7,7 +7,7 @@ class LocalParametrizationBase;
 }
 
 //Ceres includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "ceres/ceres.h"
 
 namespace wolf {
@@ -38,7 +38,7 @@ using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapp
 
 } // namespace wolf
 
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
index 7252c409d2bf0c503e06b6fe74dec895e3754150..1bdf40a64ad2320fcc597c39f65a30a1e931e8b0 100644
--- a/include/base/ceres_wrapper/solver_manager.h
+++ b/include/base/ceres_wrapper/solver_manager.h
@@ -2,8 +2,8 @@
 #define SOLVER_MANAGER_H_
 
 //wolf includes
-#include "base/wolf.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/state_block/state_block.h"
 #include "base/factor/factor_base.h"
 
 namespace wolf {
diff --git a/include/base/factory.h b/include/base/common/factory.h
similarity index 99%
rename from include/base/factory.h
rename to include/base/common/factory.h
index b161126e45c18e4cfd8a953a32b5247d58fa1d27..03b636e8104ae9e7156a9c9c3649f96f3e1a1aa4 100644
--- a/include/base/factory.h
+++ b/include/base/common/factory.h
@@ -9,7 +9,7 @@
 #define FACTORY_H_
 
 // wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 // std
 #include <string>
@@ -347,7 +347,7 @@ inline std::string LandmarkFactory::getClass()
 // Frames
 class TimeStamp;
 } // namespace wolf
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 namespace wolf{
 typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
 template<>
diff --git a/include/base/node_base.h b/include/base/common/node_base.h
similarity index 99%
rename from include/base/node_base.h
rename to include/base/common/node_base.h
index ebbc640825d3d6775da395b2fe1d02c03abf8073..7ecd7216660e39fb3ed561a500c98a2ac7977c60 100644
--- a/include/base/node_base.h
+++ b/include/base/common/node_base.h
@@ -2,7 +2,7 @@
 #define NODE_BASE_H_
 
 // Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 namespace wolf {
 
diff --git a/include/base/time_stamp.h b/include/base/common/time_stamp.h
similarity index 99%
rename from include/base/time_stamp.h
rename to include/base/common/time_stamp.h
index 02a5599af607af39d4b16bb3c05a35779cc7bb04..14ed594f6bac7d5aa47b80346691d8dd3abd828b 100644
--- a/include/base/time_stamp.h
+++ b/include/base/common/time_stamp.h
@@ -3,7 +3,7 @@
 #define TIME_STAMP_H_
 
 //wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //C, std
 #include <sys/time.h>
diff --git a/include/base/wolf.h b/include/base/common/wolf.h
similarity index 95%
rename from include/base/wolf.h
rename to include/base/common/wolf.h
index 593d4914e5b826c6068943d5f57e58d0cb5621ba..c0a9a9a2d0b7ba3342f13692b5a0beb06e30e620 100644
--- a/include/base/wolf.h
+++ b/include/base/common/wolf.h
@@ -10,8 +10,8 @@
 
 // Enable project-specific definitions and macros
 #include "internal/config.h"
-#include "base/logging.h"
 #include "base/params_server.hpp"
+#include "base/utils/logging.h"
 
 //includes from Eigen lib
 #include <Eigen/Dense>
@@ -32,19 +32,11 @@ namespace wolf {
 /**
  * \brief scalar type for the Wolf project
  *
- * This typedef makes it possible to recompile the whole Wolf project with double, float, or other precisions.
- *
- * To change the project precision, just edit wolf.h and change the precision of this typedef.
- *
  * Do NEVER forget to use Wolf scalar definitions when you code!!!
  *
  * Do NEVER use plain Eigen scalar types!!! (This is redundant with the above. But just to make sure you behave!)
- *
- * The ONLY exception to this rule is when you need special precision. The ONLY example by now is the time stamp which uses double.
  */
-//typedef float Scalar;         // Use this for float, 32 bit precision
-typedef double Scalar;        // Use this for double, 64 bit precision
-//typedef long double Scalar;   // Use this for long double, 128 bit precision
+typedef double Scalar;
 
 /**
  * \brief Vector and Matrices size type for the Wolf project
diff --git a/include/base/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
index a6448b06458658949bff014aa93da139f74f0e83..efb0837c8ad23711a2dbb72b3f3479e0db4d566a 100644
--- a/include/base/factor/factor_AHP.h
+++ b/include/base/factor/factor_AHP.h
@@ -6,7 +6,7 @@
 #include "base/landmark/landmark_AHP.h"
 #include "base/sensor/sensor_camera.h"
 //#include "base/feature/feature_point_image.h"
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
 
 #include <iomanip> //setprecision
 
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
index a3ce0e60979b6241fbf2d4f871543764430c8015..bac2e381c4426cfeb8ae1d301d1b84e14d42418a 100644
--- a/include/base/factor/factor_GPS_2D.h
+++ b/include/base/factor/factor_GPS_2D.h
@@ -3,9 +3,9 @@
 #define FACTOR_GPS_2D_H_
 
 //Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 
 namespace wolf {
     
diff --git a/include/base/factor/factor_IMU.h b/include/base/factor/factor_IMU.h
index fbb29d1642e91d9fe90f55da673f3b8a87b6c335..942cbf51c2505e05bb5daa0bf0447ddae4c7fdf1 100644
--- a/include/base/factor/factor_IMU.h
+++ b/include/base/factor/factor_IMU.h
@@ -5,7 +5,7 @@
 #include "base/feature/feature_IMU.h"
 #include "base/sensor/sensor_IMU.h"
 #include "base/factor/factor_autodiff.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 //Eigen include
 
diff --git a/include/base/factor/factor_autodiff.h b/include/base/factor/factor_autodiff.h
index be35c772aacb9c7fce4e780d413fa0df675c1655..372891cdd331da1e4837e647cebb3eda215b45b0 100644
--- a/include/base/factor/factor_autodiff.h
+++ b/include/base/factor/factor_autodiff.h
@@ -4,7 +4,7 @@
 
 //Wolf includes
 #include "base/factor/factor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 // CERES
 #include "ceres/jet.h"
diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
index d7c2091fb2c547b1511d8f5985e66723a24fb247..5b88708c4c222e79aec9497fee0d3932a8a7a01d 100644
--- a/include/base/factor/factor_autodiff_trifocal.h
+++ b/include/base/factor/factor_autodiff_trifocal.h
@@ -2,7 +2,7 @@
 #define _FACTOR_AUTODIFF_TRIFOCAL_H_
 
 //Wolf includes
-//#include "base/wolf.h"
+//#include "base/common/wolf.h"
 #include "base/factor/factor_autodiff.h"
 #include "base/sensor/sensor_camera.h"
 
@@ -118,7 +118,7 @@ class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3,
 } // namespace wolf
 
 // Includes for implentation
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf
 {
diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h
index 4562d5f75b39995fc6578b8498fc5ee95e223e61..8d79b0065fe79c24607b487d0223eceeea0f81c7 100644
--- a/include/base/factor/factor_base.h
+++ b/include/base/factor/factor_base.h
@@ -7,8 +7,8 @@ class FeatureBase;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
 
 //std includes
 
@@ -182,8 +182,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
 
 // IMPLEMENTATION //
 
-#include "base/problem.h"
-#include "base/frame_base.h"
+#include "base/problem/problem.h"
+#include "base/frame/frame_base.h"
 #include "base/feature/feature_base.h"
 #include "base/sensor/sensor_base.h"
 #include "base/landmark/landmark_base.h"
diff --git a/include/base/factor/factor_block_absolute.h b/include/base/factor/factor_block_absolute.h
index 4e0da9d764f50d5f55c14c596696b138c9b76035..3990a44123b177461519b97f8ecff84adfdd0fa9 100644
--- a/include/base/factor/factor_block_absolute.h
+++ b/include/base/factor/factor_block_absolute.h
@@ -11,7 +11,7 @@
 //Wolf includes
 #include "base/factor/factor_analytic.h"
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 
 namespace wolf {
 
diff --git a/include/base/factor/factor_container.h b/include/base/factor/factor_container.h
index 93d455a8e7e36ce85e1688c5c70a91b020d963c9..7f34ad69f30b4fc8e95216e32e53c762c8880b93 100644
--- a/include/base/factor/factor_container.h
+++ b/include/base/factor/factor_container.h
@@ -2,7 +2,7 @@
 #define FACTOR_CONTAINER_H_
 
 //Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "base/factor/factor_autodiff.h"
 #include "base/landmark/landmark_container.h"
 
diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h
index b495859c7409031153025fe507c0f10113c92a54..84f535ee73854fa9e207a41b138a98397b170ea9 100644
--- a/include/base/factor/factor_diff_drive.h
+++ b/include/base/factor/factor_diff_drive.h
@@ -10,7 +10,7 @@
 
 //Wolf includes
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 #include "base/feature/feature_diff_drive.h"
 #include "base/capture/capture_wheel_joint_position.h"
 
diff --git a/include/base/factor/factor_fix_bias.h b/include/base/factor/factor_fix_bias.h
index 6c1f321dc0f7fcaf965476dc802080e3d3bb23b5..82a13796970e40091e3f99f425a97ebbd0793e4e 100644
--- a/include/base/factor/factor_fix_bias.h
+++ b/include/base/factor/factor_fix_bias.h
@@ -6,8 +6,8 @@
 #include "base/capture/capture_IMU.h"
 #include "base/feature/feature_IMU.h"
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
 
 //#include "ceres/jet.h"
 
diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h
index e45570d79a8debe284310998cc87f48032521a10..8a9d06747b1171f89b7311f99f0a53f72ab5ca2a 100644
--- a/include/base/factor/factor_odom_2D.h
+++ b/include/base/factor/factor_odom_2D.h
@@ -3,7 +3,7 @@
 
 //Wolf includes
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 
 //#include "ceres/jet.h"
 
diff --git a/include/base/factor/factor_odom_3D.h b/include/base/factor/factor_odom_3D.h
index ce3456ef0e8c286c7b082a79a41fa268e4608a8e..c3a3cfb834ea5c74e1cdd69d804b72a9ffe8fe48 100644
--- a/include/base/factor/factor_odom_3D.h
+++ b/include/base/factor/factor_odom_3D.h
@@ -9,7 +9,7 @@
 #define FACTOR_ODOM_3D_H_
 
 #include "base/factor/factor_autodiff.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf
 {
diff --git a/include/base/factor/factor_pose_2D.h b/include/base/factor/factor_pose_2D.h
index 306b5e15ebf9e2e67eb41c8e5072aa050f7d6b32..f1cc286ead2ec56a2d45857add134b1982bbb166 100644
--- a/include/base/factor/factor_pose_2D.h
+++ b/include/base/factor/factor_pose_2D.h
@@ -4,8 +4,8 @@
 
 //Wolf includes
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
 
 //#include "ceres/jet.h"
 
diff --git a/include/base/factor/factor_pose_3D.h b/include/base/factor/factor_pose_3D.h
index b9c4da39516235081a4ec91334fa06958478264c..25273bad2c7c575b8a95d07790a4d7cd0b1fe8e7 100644
--- a/include/base/factor/factor_pose_3D.h
+++ b/include/base/factor/factor_pose_3D.h
@@ -4,8 +4,8 @@
 
 //Wolf includes
 #include "base/factor/factor_autodiff.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
 
 namespace wolf {
 
diff --git a/include/base/factor/factor_quaternion_absolute.h b/include/base/factor/factor_quaternion_absolute.h
index 1864a6c8eede4bd07190c4da737a7e1f25da117f..abb419afa0ae605265e06dedfb9ce35b9ccb75ff 100644
--- a/include/base/factor/factor_quaternion_absolute.h
+++ b/include/base/factor/factor_quaternion_absolute.h
@@ -10,9 +10,9 @@
 
 //Wolf includes
 #include "base/factor/factor_autodiff.h"
-#include "base/local_parametrization_quaternion.h"
-#include "base/frame_base.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_quaternion.h"
+#include "base/frame/frame_base.h"
+#include "base/math/rotations.h"
 
 namespace wolf {
 
diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h
index dfce11ff78a8e974731efef729c17b0fa72a9fe8..c82083332f925670c61f837fe1605a6a40d6f0ad 100644
--- a/include/base/feature/feature_IMU.h
+++ b/include/base/feature/feature_IMU.h
@@ -4,7 +4,7 @@
 //Wolf includes
 #include "base/capture/capture_IMU.h"
 #include "base/feature/feature_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //std includes
 
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index 3732bc55247d9d39b90992a07afe7bb04d7f7681..3e1ea5588f581fdaf4d395f288fb34e1e53560b7 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -8,8 +8,8 @@ class FactorBase;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
 
 //std includes
 
diff --git a/include/base/feature/feature_match.h b/include/base/feature/feature_match.h
index b96ce00239f1fb6293e3dd10b24e6acd345ae8cf..92e4560196e200083294fcad895e25fbb4c711f0 100644
--- a/include/base/feature/feature_match.h
+++ b/include/base/feature/feature_match.h
@@ -2,7 +2,7 @@
 #define FEATURE_MATCH_H_
 
 // Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //wolf nampseace
 namespace wolf {
diff --git a/include/base/frame_base.h b/include/base/frame/frame_base.h
similarity index 97%
rename from include/base/frame_base.h
rename to include/base/frame/frame_base.h
index be97eb80a333fac1391f7b19bf12d1895e9dbb6d..ffa84d74d0c7521ce226a5445e58591bfc43be66 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame/frame_base.h
@@ -10,9 +10,9 @@ class StateBlock;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/time_stamp.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/time_stamp.h"
+#include "base/common/node_base.h"
 
 //std includes
 
@@ -113,7 +113,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         void getState(Eigen::VectorXs& _state) const;
         unsigned int getSize() const;
         bool getCovariance(Eigen::MatrixXs& _cov) const;
-        Eigen::MatrixXs getCovariance() const;
 
         // Wolf tree access ---------------------------------------------------
     public:
@@ -156,10 +155,10 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
 
 // IMPLEMENTATION //
 
-#include "base/trajectory_base.h"
+#include "base/trajectory/trajectory_base.h"
 #include "base/capture/capture_base.h"
 #include "base/factor/factor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 namespace wolf {
 
diff --git a/include/base/hardware_base.h b/include/base/hardware/hardware_base.h
similarity index 91%
rename from include/base/hardware_base.h
rename to include/base/hardware/hardware_base.h
index 6dd60648b3961dd8f88244b240eebf7c4d88e37d..fb688410b73187cb3d775c00911322afdc602a86 100644
--- a/include/base/hardware_base.h
+++ b/include/base/hardware/hardware_base.h
@@ -8,8 +8,8 @@ class SensorBase;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
 
 namespace wolf {
 
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 9019ee85efd04f54ddfa920e98413c8aabf3846e..dad1e3a7c5c609fbaf5ef972f9c597f27cca201e 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -8,9 +8,9 @@ class StateBlock;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
 
 //std includes
 
@@ -72,7 +72,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         Eigen::VectorXs getState() const;
         void getState(Eigen::VectorXs& _state) const;
         bool getCovariance(Eigen::MatrixXs& _cov) const;
-        Eigen::MatrixXs getCovariance() const;
 
     protected:
         virtual void removeStateBlocks();
@@ -97,9 +96,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
 }
 
-#include "base/map_base.h"
+#include "base/map/map_base.h"
 #include "base/factor/factor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 namespace wolf{
 
diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h
index 65a2c5b710ceb0f42bb9984fee0e8d0a967348a4..122b77071f29457d933e423fd4f7a782246fc040 100644
--- a/include/base/landmark/landmark_container.h
+++ b/include/base/landmark/landmark_container.h
@@ -4,7 +4,7 @@
 
 //Wolf includes
 #include "base/landmark/landmark_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 // Std includes
 
diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h
index 3a0f84c340d12027a74bb5be7ff11f31288c06e2..7caf3b2901990d3d9bccfd0d02a51fa6f5315ced 100644
--- a/include/base/landmark/landmark_line_2D.h
+++ b/include/base/landmark/landmark_line_2D.h
@@ -4,7 +4,7 @@
 
 //Wolf includes
 #include "base/landmark/landmark_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //std includes
 
diff --git a/include/base/landmark/landmark_match.h b/include/base/landmark/landmark_match.h
index 0c5d3a4915892a3680d634f24a6a40a73df6e26c..48de1e5c99012d24ea8a6f1906284ff0135c77df 100644
--- a/include/base/landmark/landmark_match.h
+++ b/include/base/landmark/landmark_match.h
@@ -2,7 +2,7 @@
 #define LANDMARK_MATCH_H_
 
 // Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //wolf nampseace
 namespace wolf {
diff --git a/include/base/map_base.h b/include/base/map/map_base.h
similarity index 93%
rename from include/base/map_base.h
rename to include/base/map/map_base.h
index a8b447905d59c8ffb30be66cd1b91a6b36dc9ae6..c3d6eb8c92c48eb742bd1b63843c472d4f3f590c 100644
--- a/include/base/map_base.h
+++ b/include/base/map/map_base.h
@@ -9,8 +9,8 @@ class LandmarkBase;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
 
 //std includes
 
diff --git a/include/base/IMU_tools.h b/include/base/math/IMU_tools.h
similarity index 99%
rename from include/base/IMU_tools.h
rename to include/base/math/IMU_tools.h
index eecad244c7a57ee2868bcf464e951a0a5108160d..815ed2aa23098b6c0c9d372666f2c2dedae3850e 100644
--- a/include/base/IMU_tools.h
+++ b/include/base/math/IMU_tools.h
@@ -8,8 +8,8 @@
 #ifndef IMU_TOOLS_H_
 #define IMU_TOOLS_H_
 
-#include "base/wolf.h"
-#include "base/rotations.h"
+#include "base/common/wolf.h"
+#include "base/math/rotations.h"
 
 /*
  * Most functions in this file are explained in the document:
diff --git a/include/base/SE3.h b/include/base/math/SE3.h
similarity index 99%
rename from include/base/SE3.h
rename to include/base/math/SE3.h
index cea35460771f0bf91d4f56520dd823ad3a1d505d..5a84f0c7e50d55d9d56ea06cc444c8f8d33698a4 100644
--- a/include/base/SE3.h
+++ b/include/base/math/SE3.h
@@ -8,8 +8,8 @@
 #ifndef SE3_H_
 #define SE3_H_
 
-#include "base/wolf.h"
-#include "base/rotations.h"
+#include "base/common/wolf.h"
+#include "base/math/rotations.h"
 
 /*
  * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
diff --git a/include/base/pinhole_tools.h b/include/base/math/pinhole_tools.h
similarity index 99%
rename from include/base/pinhole_tools.h
rename to include/base/math/pinhole_tools.h
index 46cb1fb9dd1f12dd18d3a520cc574488e7c9f170..9fed8ba7c0775ac46d93872e98e1b2bee85fe64b 100644
--- a/include/base/pinhole_tools.h
+++ b/include/base/math/pinhole_tools.h
@@ -11,7 +11,7 @@
  *
  */
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 #include <iostream>
 
diff --git a/include/base/rotations.h b/include/base/math/rotations.h
similarity index 99%
rename from include/base/rotations.h
rename to include/base/math/rotations.h
index d4d3e711f119139e4ee2385897a12b07c60354b2..fc0e8b2bbd925c01009ec8577226cc25cfb6f0f1 100644
--- a/include/base/rotations.h
+++ b/include/base/math/rotations.h
@@ -8,7 +8,7 @@
 #ifndef ROTATIONS_H_
 #define ROTATIONS_H_
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 namespace wolf
 {
diff --git a/include/base/problem.h b/include/base/problem/problem.h
similarity index 98%
rename from include/base/problem.h
rename to include/base/problem/problem.h
index 06a574a67a8ed5dc3712b2c676082058adf9926f..d79f730cd190ae3b5137cf586873aa4c623d4485 100644
--- a/include/base/problem.h
+++ b/include/base/problem/problem.h
@@ -14,12 +14,12 @@ struct ProcessorParamsBase;
 }
 
 //wolf includes
-#include "base/wolf.h"
-#include "base/frame_base.h"
-#include "base/state_block.h"
 #include "base/params_server.hpp"
 #include "base/sensor/new_sensor_factory.h"
 #include "base/processor/new_processor_factory.h"
+#include "base/common/wolf.h"
+#include "base/frame/frame_base.h"
+#include "base/state_block/state_block.h"
 
 // std includes
 
@@ -251,10 +251,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
         bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
         bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
-        Eigen::MatrixXs getFrameCovariance(FrameBaseConstPtr _frame_ptr);
-        Eigen::MatrixXs getLastKeyFrameCovariance();
+        bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
         bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
-        Eigen::MatrixXs getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr);
 
         // Solver management ----------------------------------------
 
diff --git a/include/base/diff_drive_tools.h b/include/base/processor/diff_drive_tools.h
similarity index 99%
rename from include/base/diff_drive_tools.h
rename to include/base/processor/diff_drive_tools.h
index 142b8c5022ccca18eaf1edc1214984b93bd816c4..92883eafa55af400e948f9b637389255b035eef0 100644
--- a/include/base/diff_drive_tools.h
+++ b/include/base/processor/diff_drive_tools.h
@@ -8,7 +8,7 @@
 #ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
 #define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
 
-#include "base/eigen_assert.h"
+#include "base/utils/eigen_assert.h"
 
 namespace wolf {
 
@@ -419,6 +419,6 @@ Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov(
 
 } // namespace wolf
 
-#include "base/diff_drive_tools.hpp"
+#include "base/processor/diff_drive_tools.hpp"
 
 #endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/include/base/diff_drive_tools.hpp b/include/base/processor/diff_drive_tools.hpp
similarity index 100%
rename from include/base/diff_drive_tools.hpp
rename to include/base/processor/diff_drive_tools.hpp
diff --git a/include/base/motion_buffer.h b/include/base/processor/motion_buffer.h
similarity index 98%
rename from include/base/motion_buffer.h
rename to include/base/processor/motion_buffer.h
index f8d0ad5be8e854eaf8badca494218b637795d16e..40fbd29251a47338412a3e2a307917dc1d0b5f78 100644
--- a/include/base/motion_buffer.h
+++ b/include/base/processor/motion_buffer.h
@@ -8,8 +8,8 @@
 #ifndef SRC_MOTIONBUFFER_H_
 #define SRC_MOTIONBUFFER_H_
 
-#include "base/wolf.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/time_stamp.h"
 
 #include <list>
 #include <algorithm>
diff --git a/include/base/processor/new_processor_factory.h b/include/base/processor/new_processor_factory.h
index 4e925414ec311faebc7aa47d9d91c0c57808a4f7..fc6c2bc8c727e5fa318b3c0c5d8291f4f6575fa3 100644
--- a/include/base/processor/new_processor_factory.h
+++ b/include/base/processor/new_processor_factory.h
@@ -15,7 +15,7 @@ struct ProcessorParamsBase;
 }
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // std
 
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index e996a382605975ccda456320961ab775e3593583..aa6aaf7ac5b0bbfb8e0388cf3a2bcdcdf590c79b 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,11 +7,11 @@ class SensorBase;
 }
 
 // Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
-#include "base/frame_base.h"
 #include "base/params_server.hpp"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
+#include "base/frame/frame_base.h"
 
 // std
 #include <memory>
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index d93e6d83f0ef546f8dacb7f96d372c5ab6fa11a7..a486c721865fc21ed05135ccfb8d1717cfa738a9 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -9,8 +9,8 @@
 #define _WOLF_PROCESSOR_DIFF_DRIVE_H_
 
 #include "base/processor/processor_motion.h"
-#include "base/diff_drive_tools.h"
 #include "base/params_server.hpp"
+#include "base/processor/diff_drive_tools.h"
 
 namespace wolf {
 
diff --git a/include/base/processor/processor_factory.h b/include/base/processor/processor_factory.h
index 8077aae495c053e229fc8fef08790f1ca6e3b099..390a66bec44b331094245686763cec31416fdb00 100644
--- a/include/base/processor/processor_factory.h
+++ b/include/base/processor/processor_factory.h
@@ -15,7 +15,7 @@ struct ProcessorParamsBase;
 }
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // std
 
@@ -155,7 +155,7 @@ namespace wolf
  *     \code
  *     #include "base/sensor/sensor_odom_2D.h"
  *     #include "base/processor/processor_odom_2D.h"
- *     #include "base/problem.h"
+ *     #include "base/problem/problem.h"
  *
  *     Problem problem(FRM_PO_2D);
  *     problem.installSensor    ( "ODOM 2D" , "Main odometer" , extrinsics      , &intrinsics );
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
index 9a66768a99a28ebfd32bfc1346f613020c11d848..619b88b7990cf365fa2616546be2d88821bac528 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -3,8 +3,8 @@
 
 // Wolf related headers
 #include "base/processor/processor_loopclosure_base.h"
-#include "base/state_block.h"
 #include "base/params_server.hpp"
+#include "base/state_block/state_block.h"
 
 namespace wolf{
 
diff --git a/include/base/processor/processor_logging.h b/include/base/processor/processor_logging.h
index 4add7a8e9d7945f0aaab54c8e7e7f1acdc4bfd8e..d41d7ce30af37063f429a979aeb41a780b2ab489 100644
--- a/include/base/processor/processor_logging.h
+++ b/include/base/processor/processor_logging.h
@@ -9,7 +9,7 @@
 #define _WOLF_PROCESSOR_LOGGING_H_
 
 /// @brief un-comment for IDE highlights.
-//#include "base/logging.h"
+//#include "base/utils/logging.h"
 
 #define __INTERNAL_WOLF_ASSERT_PROCESSOR \
   static_assert(std::is_base_of<ProcessorBase, \
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 5700398ad372524c391e4e8cffab5255d8239d64..805ef9261d3b6b76c9df1afe1a2631ba64162b5d 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -11,8 +11,8 @@
 // Wolf
 #include "base/capture/capture_motion.h"
 #include "base/processor/processor_base.h"
-#include "base/time_stamp.h"
 #include "base/params_server.hpp"
+#include "base/common/time_stamp.h"
 
 // std
 #include <iomanip>
@@ -486,7 +486,7 @@ class ProcessorMotion : public ProcessorBase
 
 }
 
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 
 namespace wolf{
 
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index ed4fcea1d667a35944bb90f9fded98cfd28f180e..5f4718c233f3ae9db651b661a9f146f87ba67116 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -11,8 +11,8 @@
 #include "base/processor/processor_motion.h"
 #include "base/capture/capture_odom_2D.h"
 #include "base/factor/factor_odom_2D.h"
-#include "base/rotations.h"
 #include "base/params_server.hpp"
+#include "base/math/rotations.h"
 
 namespace wolf {
 
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 14c547aacf7e238d4d27860b862a45238f248001..f55d4175906224157e65c389acac4010e6c34aa9 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -12,7 +12,7 @@
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/capture/capture_odom_3D.h"
 #include "base/factor/factor_odom_3D.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 #include <cmath>
 
 namespace wolf {
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index 08620778d8375da9fa63f8b5bedfa6b03d89fb73..ea414cfffc484b73e1025f8e32aa520e4a7ffeb0 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -17,8 +17,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
 
 struct ProcessorParamsTracker : public ProcessorParamsBase
 {
-        unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
-        unsigned int max_new_features;
+    unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
     ProcessorParamsTracker() = default;
     ProcessorParamsTracker(std::string _unique_name, const wolf::paramsServer & _server):
         ProcessorParamsBase(_unique_name, _server)
@@ -26,6 +25,7 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
         min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
         max_new_features = _server.getParam<unsigned int>(_unique_name + "/max_new_features", "10");
     }
+    }
 };
 
 WOLF_PTR_TYPEDEFS(ProcessorTracker);
@@ -188,7 +188,7 @@ class ProcessorTracker : public ProcessorBase
         /**\brief Process new Features or Landmarks
          *
          */
-        virtual unsigned int processNew(const unsigned int& _max_features) = 0;
+        virtual unsigned int processNew(const int& _max_features) = 0;
 
         /**\brief Creates and adds factors from last_ to origin_
          *
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index 2d244d05aa08c38a877fadbe5f39bd203e055cd8..e73766aee132602e94bd16d01a8c6129ec26890d 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -12,8 +12,8 @@
 #include "base/processor/processor_tracker.h"
 #include "base/capture/capture_base.h"
 #include "base/feature/feature_match.h"
-#include "base/track_matrix.h"
-#include "base/wolf.h"
+#include "base/processor/track_matrix.h"
+#include "base/common/wolf.h"
 
 namespace wolf
 {
@@ -143,17 +143,19 @@ class ProcessorTrackerFeature : public ProcessorTracker
         /**\brief Process new Features
          *
          */
-        virtual unsigned int processNew(const unsigned int& _max_features);
+        virtual unsigned int processNew(const int& _max_features);
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features detected
+         * \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 sets the member new_features_last_, the list of newly detected features.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) = 0;
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) = 0;
 
         /** \brief Create a new factor and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 3ae1d8d38ec88d7284fb31a749ee01fefe34f3a9..5ab8b2027d35a29bce529103a9ecf522a4d527f0 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -14,7 +14,7 @@
 #include "base/feature/feature_corner_2D.h"
 #include "base/landmark/landmark_corner_2D.h"
 #include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/association/association_tree.h"
 #include "base/processor/processor_tracker_feature.h"
 #include "base/params_server.hpp"
@@ -107,16 +107,16 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+         * \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 sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 586cd57d716519a45f01f66050f39f2bb45f6bee..c0b40edbea015ffb86971363c218659c21d67af7 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -8,7 +8,7 @@
 #ifndef PROCESSOR_TRACKER_FEATURE_DUMMY_H_
 #define PROCESSOR_TRACKER_FEATURE_DUMMY_H_
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "base/processor/processor_tracker_feature.h"
 #include "base/factor/factor_epipolar.h"
 
@@ -56,16 +56,16 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+         * \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 sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 800ca1211fbdcdd3d8f99781be90d4ddfdbfad11..7378c8a3d67596e5357a2e40f5f5475f00a21c42 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -5,8 +5,8 @@
 #include "base/sensor/sensor_camera.h"
 #include "base/capture/capture_image.h"
 #include "base/feature/feature_point_image.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "base/processor/processor_tracker_feature.h"
 #include "base/factor/factor_epipolar.h"
 #include "base/processor/processor_params_image.h"
@@ -112,14 +112,16 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
         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 is intended to create Features that are not among the Features already known in the Map.
-         *
-         * This function sets new_features_last_, the list of newly detected features.
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * \return The number of detected Features.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index fa3984253c94434b029bd408e516824540e285c8..a017004cdfa9634a4b36b31926002134a3931160 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -98,14 +98,16 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame() override;
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features detected
+         * \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 sets the member new_features_last_, the list of newly detected features.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
 
         /** \brief Create a new factor and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index 6a68ce1bb66950f0a089d6df25ae3b465226c08d..c6db221307f24ba50592f86fd12fb1be8c66843d 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -129,18 +129,19 @@ class ProcessorTrackerLandmark : public ProcessorTracker
         /** \brief Process new Features
          *
          */
-        unsigned int processNew(const unsigned int& _max_features);
+        unsigned int processNew(const int& _max_features);
 
         /** \brief Detect new Features
-         * \param _max_features The maximum number of features to detect.
+         * \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 sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * 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 unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
 
         /** \brief Creates a landmark for each of new_features_last_
          **/
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index a25a2646f21efd437201b8cac0c0899164a07597..2b381719af5c7561f9d721d99d990cdf0ae1bdba 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -14,7 +14,7 @@
 #include "base/feature/feature_corner_2D.h"
 #include "base/landmark/landmark_corner_2D.h"
 #include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/association/association_tree.h"
 #include "base/processor/processor_tracker_landmark.h"
 
@@ -135,16 +135,16 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+         * \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 sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * 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 unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index 82b446c47d15b282d985a5d6020a9f33c9d79936..c0c3424cb0983f352d6976a7bee0ec07af9bdaa2 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -48,15 +48,16 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features to detect.
+         * \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 sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * 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 unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index d58920cc10b943c74a27dd1c3f61946624747133..f98f87707a9821b3082dabb79039f2f25018966c 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -7,7 +7,7 @@
 #include "base/landmark/landmark_match.h"
 #include "base/processor/processor_params_image.h"
 #include "base/processor/processor_tracker_landmark.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 #include <algorithms/activesearch/alg_activesearch.h>
 #include <descriptors/descriptor_base.h>
@@ -128,14 +128,16 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
         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 is intended to create Features that are not among the Features already known in the Map.
-         *
-         * This function sets new_features_last_, the list of newly detected features.
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * \return The number of detected Features.
+         * 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 unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/track_matrix.h b/include/base/processor/track_matrix.h
similarity index 100%
rename from include/base/track_matrix.h
rename to include/base/processor/track_matrix.h
diff --git a/include/base/sensor/new_sensor_factory.h b/include/base/sensor/new_sensor_factory.h
index 46684a7762248457a088b8da3d672fd0bd2855c9..b84afb0ec0b1140da7592a3dfe53573e56088c96 100644
--- a/include/base/sensor/new_sensor_factory.h
+++ b/include/base/sensor/new_sensor_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
 }
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 namespace wolf
 {
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index 14f937ead64e684777498346e1bf1d0ab1fd94cb..5734a295fe10a1c306988a66c92b3a9a2113fb13 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -11,7 +11,7 @@
 //wolf
 #include "base/sensor/sensor_base.h"
 //#include "sensor_factory.h"
-//#include "base/factory.h"
+//#include "base/common/factory.h"
 
 // std
 
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index b4f5e233ba5db866a885b84ac35f9ae3002581cd..e7612d14998adf0e74f0ca868b7875350a67a228 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -9,9 +9,9 @@ class StateBlock;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
-#include "base/time_stamp.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
+#include "base/common/time_stamp.h"
 
 //std includes
 
@@ -191,8 +191,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
 
 }
 
-#include "base/problem.h"
-#include "base/hardware_base.h"
+#include "base/problem/problem.h"
+#include "base/hardware/hardware_base.h"
 #include "base/capture/capture_base.h"
 #include "base/processor/processor_base.h"
 
diff --git a/include/base/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
index 2f8a13f0e88090c8fcf86f608ef6a30e000c1840..fd0a801105af5b0fe7f4707fa1088b63ccc38efd 100644
--- a/include/base/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -10,8 +10,8 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/diff_drive_tools.h"
 #include "base/params_server.hpp"
+#include "base/processor/diff_drive_tools.h"
 
 namespace wolf {
 
diff --git a/include/base/sensor/sensor_factory.h b/include/base/sensor/sensor_factory.h
index 32884c18f0b7b1e3133648ebb946d216898a8f56..d74c9b1abb4be615f02323efb2bcec33e32c1dcb 100644
--- a/include/base/sensor/sensor_factory.h
+++ b/include/base/sensor/sensor_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
 }
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 namespace wolf
 {
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index f64b3571970400704e9ea680e291b705362cdebc..28e43acfa63426072e6883b2a3237535775d1fc8 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -2,8 +2,8 @@
 #define _WOLF_SOLVER_MANAGER_H_
 
 //wolf includes
-#include "base/wolf.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/state_block/state_block.h"
 #include "base/factor/factor_base.h"
 
 namespace wolf {
diff --git a/include/base/solver_suitesparse/cost_function_base.h b/include/base/solver_suitesparse/cost_function_base.h
index 8aae707aed16ec7229c3d4a6730a148d6492c785..25d891b953afd62035fe8b8d4d3047f038e34db4 100644
--- a/include/base/solver_suitesparse/cost_function_base.h
+++ b/include/base/solver_suitesparse/cost_function_base.h
@@ -8,7 +8,7 @@
 #ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
 #define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include <Eigen/StdVector>
 
 class CostFunctionBase
diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h
index bdcde3834cc43b950fd968a1c559ee778365e0b0..3ed0eb5e352692eb6af4511f0414f6204f914b33 100644
--- a/include/base/solver_suitesparse/cost_function_sparse.h
+++ b/include/base/solver_suitesparse/cost_function_sparse.h
@@ -2,7 +2,7 @@
 #define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
 
 //wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "cost_function_sparse_base.h"
 
 // CERES JET
diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h
index 9aeff876c9134363b1c95055f4fbef9b1d87a740..d90a0577ead23d37aaa6f6a37c0b92c5aa06f89f 100644
--- a/include/base/solver_suitesparse/cost_function_sparse_base.h
+++ b/include/base/solver_suitesparse/cost_function_sparse_base.h
@@ -9,7 +9,7 @@
 #define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
 
 //wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "cost_function_base.h"
 
 // CERES JET
diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h
index 154b78639d03c71368b65bbaf65ee778421ba313..0d7f49ccd037785f4b409462fc072127dc36d26e 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -13,7 +13,7 @@
 #include <ctime>
 
 //Wolf includes
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "../factor_sparse.h"
 #include "base/factor/factor_odom_2D.h"
 #include "base/factor/factor_corner_2D.h"
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
index 09c0abe7f8a83cde800e76968da2a5a990e0007c..3d1ef2a276a95526881859da5fcee019308d2e76 100644
--- a/include/base/solver_suitesparse/solver_manager.h
+++ b/include/base/solver_suitesparse/solver_manager.h
@@ -3,8 +3,8 @@
 
 //wolf includes
 #include "base/factor/factor_GPS_2D.h"
-#include "base/wolf.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/state_block/state_block.h"
 #include "../state_point.h"
 #include "../state_complex_angle.h"
 #include "../state_theta.h"
diff --git a/include/base/local_parametrization_angle.h b/include/base/state_block/local_parametrization_angle.h
similarity index 95%
rename from include/base/local_parametrization_angle.h
rename to include/base/state_block/local_parametrization_angle.h
index 912237c07a8b4a7aa4d711c39c68f3c9cccf8d24..3644c83ec487f6ca4c21816d32f3e0ed87535b94 100644
--- a/include/base/local_parametrization_angle.h
+++ b/include/base/state_block/local_parametrization_angle.h
@@ -8,8 +8,8 @@
 #ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
 #define LOCAL_PARAMETRIZATION_ANGLE_H_
 
-#include "base/local_parametrization_base.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_base.h"
+#include "base/math/rotations.h"
 
 namespace wolf
 {
diff --git a/include/base/local_parametrization_base.h b/include/base/state_block/local_parametrization_base.h
similarity index 97%
rename from include/base/local_parametrization_base.h
rename to include/base/state_block/local_parametrization_base.h
index 3b7dcc38dc698936a707101aa40cad076adc910f..cbc0cbb13aff8535d46926530b99f654714f3832 100644
--- a/include/base/local_parametrization_base.h
+++ b/include/base/state_block/local_parametrization_base.h
@@ -8,7 +8,7 @@
 #ifndef LOCAL_PARAMETRIZATION_BASE_H_
 #define LOCAL_PARAMETRIZATION_BASE_H_
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 namespace wolf {
 
diff --git a/include/base/local_parametrization_homogeneous.h b/include/base/state_block/local_parametrization_homogeneous.h
similarity index 97%
rename from include/base/local_parametrization_homogeneous.h
rename to include/base/state_block/local_parametrization_homogeneous.h
index f06dd2fb150e5f34eede487e3a685fb88f00d130..cd2076d635ec2f674fb42b16bf000a689700b27c 100644
--- a/include/base/local_parametrization_homogeneous.h
+++ b/include/base/state_block/local_parametrization_homogeneous.h
@@ -8,7 +8,7 @@
 #ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
 #define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
 
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/local_parametrization_polyline_extreme.h b/include/base/state_block/local_parametrization_polyline_extreme.h
similarity index 95%
rename from include/base/local_parametrization_polyline_extreme.h
rename to include/base/state_block/local_parametrization_polyline_extreme.h
index 3a75a8a0ff4eede5a3e865810555852653494713..0e0cc29cd9ab50b54cb09cde36a12d278e1a846a 100644
--- a/include/base/local_parametrization_polyline_extreme.h
+++ b/include/base/state_block/local_parametrization_polyline_extreme.h
@@ -8,7 +8,7 @@
 #ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
 #define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
 
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/local_parametrization_quaternion.h b/include/base/state_block/local_parametrization_quaternion.h
similarity index 97%
rename from include/base/local_parametrization_quaternion.h
rename to include/base/state_block/local_parametrization_quaternion.h
index f92676558d4cbd8da107852779241a20836c2fe0..c28a05d49d6e1cd8548161e5597393817c14c03f 100644
--- a/include/base/local_parametrization_quaternion.h
+++ b/include/base/state_block/local_parametrization_quaternion.h
@@ -8,7 +8,7 @@
 #ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
 #define LOCAL_PARAMETRIZATION_QUATERNION_H_
 
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/state_angle.h b/include/base/state_block/state_angle.h
similarity index 84%
rename from include/base/state_angle.h
rename to include/base/state_block/state_angle.h
index c61286e13eb4fcaa82c70c1c8db8784decfb1b86..ae5133b3261212958e3f418757092562ae162457 100644
--- a/include/base/state_angle.h
+++ b/include/base/state_block/state_angle.h
@@ -8,8 +8,8 @@
 #ifndef STATE_ANGLE_H_
 #define STATE_ANGLE_H_
 
-#include "base/state_block.h"
-#include "base/local_parametrization_angle.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/local_parametrization_angle.h"
 
 namespace wolf
 {
diff --git a/include/base/state_block.h b/include/base/state_block/state_block.h
similarity index 98%
rename from include/base/state_block.h
rename to include/base/state_block/state_block.h
index 447e8a0e9b3534f8048f3aa55131bdaed617e0d7..86c94d6ce3c36a2ab84fe3101749e76ca47f5e92 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block/state_block.h
@@ -9,7 +9,7 @@ class LocalParametrizationBase;
 }
 
 //Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //std includes
 #include <iostream>
@@ -154,9 +154,9 @@ public:
 } // namespace wolf
 
 // IMPLEMENTATION
-#include "base/local_parametrization_base.h"
-#include "base/node_base.h"
-#include "base/problem.h"
+#include "base/state_block/local_parametrization_base.h"
+#include "base/common/node_base.h"
+#include "base/problem/problem.h"
 
 namespace wolf {
 
diff --git a/include/base/state_homogeneous_3D.h b/include/base/state_block/state_homogeneous_3D.h
similarity index 91%
rename from include/base/state_homogeneous_3D.h
rename to include/base/state_block/state_homogeneous_3D.h
index adfda9018041d3a9d2469dc0aa16ea7e93ccdfe0..369e14f4308817d5db319d8ed86d26fee8fecbec 100644
--- a/include/base/state_homogeneous_3D.h
+++ b/include/base/state_block/state_homogeneous_3D.h
@@ -8,8 +8,8 @@
 #ifndef SRC_STATE_HOMOGENEOUS_3D_H_
 #define SRC_STATE_HOMOGENEOUS_3D_H_
 
-#include "base/state_block.h"
-#include "base/local_parametrization_homogeneous.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/local_parametrization_homogeneous.h"
 
 namespace wolf {
 
diff --git a/include/base/state_quaternion.h b/include/base/state_block/state_quaternion.h
similarity index 92%
rename from include/base/state_quaternion.h
rename to include/base/state_block/state_quaternion.h
index d990ce1f8c6ed89c3150e4958d7ad570b1e2f606..f7ad39f2d05186b4925618d6422f989288e2956e 100644
--- a/include/base/state_quaternion.h
+++ b/include/base/state_block/state_quaternion.h
@@ -8,8 +8,8 @@
 #ifndef SRC_STATE_QUATERNION_H_
 #define SRC_STATE_QUATERNION_H_
 
-#include "base/state_block.h"
-#include "base/local_parametrization_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/local_parametrization_quaternion.h"
 
 namespace wolf {
 
diff --git a/include/base/trajectory_base.h b/include/base/trajectory/trajectory_base.h
similarity index 96%
rename from include/base/trajectory_base.h
rename to include/base/trajectory/trajectory_base.h
index 6ceddf9e6a8742133b01ee9277369a0a3bcaa5be..37f9762c7c9955381eaba9bc52aef253441d36e1 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory/trajectory_base.h
@@ -10,8 +10,8 @@ class TimeStamp;
 }
 
 //Wolf includes
-#include "base/wolf.h"
-#include "base/node_base.h"
+#include "base/common/wolf.h"
+#include "base/common/node_base.h"
 
 //std includes
 
diff --git a/include/base/eigen_assert.h b/include/base/utils/eigen_assert.h
similarity index 100%
rename from include/base/eigen_assert.h
rename to include/base/utils/eigen_assert.h
diff --git a/include/base/eigen_predicates.h b/include/base/utils/eigen_predicates.h
similarity index 99%
rename from include/base/eigen_predicates.h
rename to include/base/utils/eigen_predicates.h
index 55f2eeeb75df3360e504105bd6cb08660b1cc5ea..bc0ab932275487669cd335140c5b22eb0a7611ad 100644
--- a/include/base/eigen_predicates.h
+++ b/include/base/utils/eigen_predicates.h
@@ -8,7 +8,7 @@
 #ifndef _WOLF_EIGEN_PREDICATES_H_
 #define _WOLF_EIGEN_PREDICATES_H_
 
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf {
 
diff --git a/include/base/logging.h b/include/base/utils/logging.h
similarity index 99%
rename from include/base/logging.h
rename to include/base/utils/logging.h
index 3593fc718de7b717db2d19f588fe8e97a1e814ce..088a82152f5eeef569d3e71919924d1ef5e4d511 100644
--- a/include/base/logging.h
+++ b/include/base/utils/logging.h
@@ -17,7 +17,7 @@
 #include "spdlog/fmt/bundled/ostream.h"
 
 // Wolf includes
-#include "base/singleton.h"
+#include "base/utils/singleton.h"
 
 namespace wolf {
 namespace internal {
diff --git a/include/base/make_unique.h b/include/base/utils/make_unique.h
similarity index 100%
rename from include/base/make_unique.h
rename to include/base/utils/make_unique.h
diff --git a/include/base/singleton.h b/include/base/utils/singleton.h
similarity index 100%
rename from include/base/singleton.h
rename to include/base/utils/singleton.h
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
index 0c38a5a6126723e4e61d0bd15a7616a1b24f7c4c..1385e1af088678e853c0ab0ecb4f5d6a34f103a0 100644
--- a/serialization/cereal/serialization_local_parametrization_base.h
+++ b/serialization/cereal/serialization_local_parametrization_base.h
@@ -1,7 +1,7 @@
 #ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
 #define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
 
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
 
 #include <cereal/cereal.hpp>
 #include <cereal/types/polymorphic.hpp>
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
index 6dbc07d59d21a3b33051d270825be9da060366c7..8bfc7af0489e3639c2c9dbb244faaf3804d110dd 100644
--- a/src/capture/capture_IMU.cpp
+++ b/src/capture/capture_IMU.cpp
@@ -1,6 +1,6 @@
 #include "base/capture/capture_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf {
 
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
index 7b11d9c8de9e6189be3c9b6a4bef5864b392e9f6..1531820a06d63e478ff533cc0c47e42b582bf486 100644
--- a/src/capture/capture_wheel_joint_position.cpp
+++ b/src/capture/capture_wheel_joint_position.cpp
@@ -1,6 +1,6 @@
 #include "base/capture/capture_wheel_joint_position.h"
 #include "base/sensor/sensor_diff_drive.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf {
 
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 95ca1a65631b33e9322485caaf6204af84ae5833..2b9b1bc6a52fe7556488000539fa5da2ff88bb84 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,9 +1,9 @@
 #include "base/ceres_wrapper/ceres_manager.h"
 #include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
 #include "base/landmark/landmark_base.h"
-#include "base/make_unique.h"
+#include "base/utils/make_unique.h"
 
 namespace wolf {
 
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index 06ae2d113c1a4c4a988fa6a684be614c90ed4d9a..72e6e556be97e476e6d57175338790632ef78098 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -1,6 +1,6 @@
 #include "base/solver/solver_manager.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
 #include "base/landmark/landmark_base.h"
 
 namespace wolf {
diff --git a/src/node_base.cpp b/src/common/node_base.cpp
similarity index 75%
rename from src/node_base.cpp
rename to src/common/node_base.cpp
index 24cb56c200843b2c816cb243de2096b9ebcd7f98..882e70a6944ba1b268262004dc46af33707b3221 100644
--- a/src/node_base.cpp
+++ b/src/common/node_base.cpp
@@ -1,4 +1,4 @@
-#include "base/node_base.h"
+#include "base/common/node_base.h"
 
 namespace wolf {
 
diff --git a/src/time_stamp.cpp b/src/common/time_stamp.cpp
similarity index 97%
rename from src/time_stamp.cpp
rename to src/common/time_stamp.cpp
index 55d25fc6ed7fc8d67d609b2d1f7d16d1455aef1b..3e84bf8460686af0fea308d8897966d8934d008e 100644
--- a/src/time_stamp.cpp
+++ b/src/common/time_stamp.cpp
@@ -1,5 +1,5 @@
 
-#include "base/time_stamp.h"
+#include "base/common/time_stamp.h"
 
 namespace wolf {
 
diff --git a/src/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
index 2fae213bcb4493647a2e35b963af6c89bdb34dfc..d9eec5b00f275b9ed4fc1fd2f309a73daa8a171e 100644
--- a/src/examples/solver/test_ccolamd.cpp
+++ b/src/examples/solver/test_ccolamd.cpp
@@ -6,7 +6,7 @@
  */
 
 // Wolf includes
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //std includes
 #include <cstdlib>
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index 93b39fe6572185da38ef7fccb8cb48b6302bab1c..dc4094304626dda2d5f044576da3cbaed648d377 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -17,7 +17,7 @@
 #include <queue>
 
 //Wolf includes
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/factor/factor_base.h"
 #include "base/sensor/sensor_laser_2D.h"
 #include "wolf_manager.h"
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index 10824d51e65440e25dd4a2e5b93b810d5f0a2372..9174dc94f6d30e66ae586d97ecc53896e0f2522a 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -17,7 +17,7 @@
 #include "glog/logging.h"
 
 //Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/processor/processor_tracker_landmark_corner.h"
 #include "base/processor/processor_odom_2D.h"
 #include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index d4afdc7d00f1805782648bf826159d5ea955d288..d12f491de7ce0895ca301498454e7f3d451bbd4e 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -17,7 +17,7 @@
 #include "glog/logging.h"
 
 //Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/processor/processor_tracker_landmark_corner.h"
 #include "base/processor/processor_odom_2D.h"
 #include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 9e9ca5390a97a42ec61f7ad2a76081fa40e72b5c..136ba285e0d9fb2c62b4801dc75e597dcdd257ff 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -17,7 +17,7 @@
 #include "glog/logging.h"
 
 //Wolf includes
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/processor/processor_tracker_landmark_polyline.h"
 #include "base/processor/processor_odom_2D.h"
 #include "base/sensor/sensor_laser_2D.h"
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 306c7dfdc0fae360e7ec0d68a763cebf278d4182..23980071f58e0e30bc3eeebef8c6d51106e2adf2 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -6,8 +6,8 @@
  */
 
 //Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_diff_drive.h"
 #include "base/capture/capture_wheel_joint_position.h"
 #include "base/processor/processor_diff_drive.h"
diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
index 1c01f5e1361a9ee43388a03c39928a405342f244..d24a715cbc8d65f2b86d67e3931b9c1199f22398 100644
--- a/src/examples/test_eigen_quaternion.cpp
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -6,7 +6,7 @@
 #include <eigen3/Eigen/Geometry>
 
 //Wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 int main()
 {
diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp
index b3938285e114b46926da4d5b18255e10e4130be6..75031dff808230fb6ff00b8633699960a6e361e6 100644
--- a/src/examples/test_factor_AHP.cpp
+++ b/src/examples/test_factor_AHP.cpp
@@ -1,8 +1,8 @@
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
 #include "base/landmark/landmark_AHP.h"
 #include "base/factor/factor_AHP.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "base/sensor/sensor_camera.h"
 #include "base/capture/capture_image.h"
 
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index 5dde0e16a573a9b8e4e5fb90d6b65e905dcb18e6..3d0483e9647f9e3203ab3bb89036de55e0f9adc9 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -3,11 +3,11 @@
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
 #include "base/capture/capture_pose.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
 //#define DEBUG_RESULTS
diff --git a/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
index 621f651866200d68a0b3061cb699c6f56d896954..e095e561d3348cb3ff4445c5f874e8f34ca637e9 100644
--- a/src/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -16,10 +16,10 @@
 #include "unistd.h"
 
 // wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "base/feature/feature_base.h"
 #include "base/landmark/landmark_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 //faramotics includes
 #include "faramotics/dynamicSceneRender.h"
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index dff0efd51fe4a71d8d9dc22b6630f4b50fa62946..dab6894d7a558cf53502ffc58c6a407a3fa4bbe5 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -7,8 +7,8 @@
 
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/processor/processor_odom_3D.h"
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 4415b685cecace3b3aa348d6ca4adfd748219acd..43a54d654c1611e7d9c5a42ef326c382f2dd9566 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -7,8 +7,8 @@
 
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/processor/processor_odom_3D.h"
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index e7ed6a9157f104629a2d6f41d4277e4450763672..4b923de89990eac72ffc20df61b13e28dbb76b66 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -2,12 +2,12 @@
 #include "base/capture/capture_IMU.h"
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "base/processor/processor_odom_3D.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index f134ccc124603660a1d687fe5fa3357427677415..e542e18e0ffac35b9c398ba01ab23bd7fc2e13a1 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -2,12 +2,12 @@
 #include "base/capture/capture_IMU.h"
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/factor/factor_odom_3D.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "base/processor/processor_odom_3D.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index 7dedb1200e1fcfa14483a848bf5b3032aac3ccd5..2dd5ab64d964e3621eb671d03ef5e8cf179256cc 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -5,12 +5,12 @@
  *      \author: jsola
  */
 
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/map_base.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/map/map_base.h"
 #include "base/landmark/landmark_polyline_2D.h"
 #include "base/landmark/landmark_AHP.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/yaml/yaml_conversion.h"
 
 #include <iostream>
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index 0e397cac34729accf6ecd24da3829f011044800a..1fbed214fb99a53c9c4d509ccc3e47d27c744cb1 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -7,10 +7,10 @@
 
  //Wolf
 #include "base/capture/capture_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 7dc68ebd7d36b75ee7dc15299495ede4bc6f8fe2..33ec4cabb2580353719cf4d50ed14976e626cfaa 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -9,10 +9,10 @@
 #include "base/capture/capture_IMU.h"
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
index 6cf92cef243dde6230b39220224c1a2a87b18724..22c797d6d0726e600427680bd475dede67ebc5b8 100644
--- a/src/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -9,10 +9,10 @@
 #include "base/capture/capture_IMU.h"
 #include "base/sensor/sensor_IMU.h"
 #include <test/processor_IMU_UnitTester.h>
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index ffeb5ce92272894a19ec5c7161ee354412a7072a..79798150de9d17ece60fbcec7020e5693080d6b6 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -6,10 +6,10 @@
  */
 
 #include "base/capture/capture_IMU.h"
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_odom_2D.h"
 #include "base/processor/processor_odom_3D.h"
-#include "base/map_base.h"
+#include "base/map/map_base.h"
 #include "base/landmark/landmark_base.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index ab22be41878fa14bdd84425ce150b30008834696..77e2b35e9956b01622d81f8ca1e7cdae0b60c25e 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -9,10 +9,10 @@
 #include <iostream>
 
 //Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/processor/processor_tracker_feature_dummy.h"
 #include "base/capture/capture_void.h"
 
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index 81900c7ef6077fe947f09c9f59a42d094c268a40..1702229762a5aa413678f6937a8e78d4b7b921df 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -9,10 +9,10 @@
 #include <iostream>
 
 //Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/processor/processor_tracker_landmark_dummy.h"
 #include "base/capture/capture_void.h"
 
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index d8b22c18d234ff658b22179060a6008ef663e117..be7df536e971273559252aeff8dc51bd808bb14b 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -4,9 +4,9 @@
 #include "base/processor/processor_tracker_landmark_image.h"
 
 //Wolf
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
 #include "base/processor/processor_odom_3D.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/sensor/sensor_camera.h"
diff --git a/src/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
index c24e8904eeaf06490b0fa3283e52cd074f1bcf70..b9f01912d4341df33d3fd270ef8d66c9e15f4477 100644
--- a/src/examples/test_projection_points.cpp
+++ b/src/examples/test_projection_points.cpp
@@ -6,7 +6,7 @@
 #include <iostream>
 
 //wolf includes
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
 
 int main(int argc, char** argv)
 {
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 142b9980846a38c123e01a61c4df2513f724729e..6bd1805dc78a0c65bd27ca022fbb46bd272cf6d4 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -5,11 +5,11 @@
  *      \author: jtarraso
  */
 
-#include "base/wolf.h"
-#include "base/frame_base.h"
-#include "base/pinhole_tools.h"
+#include "base/common/wolf.h"
+#include "base/frame/frame_base.h"
+#include "base/math/pinhole_tools.h"
 #include "base/sensor/sensor_camera.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 #include "base/capture/capture_image.h"
 #include "base/landmark/landmark_AHP.h"
 #include "base/factor/factor_AHP.h"
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index e046765f0bd18cb289a4137f452c581440ab08c8..56982c9dc7c6fda44b594a04f92a33f911b8ca49 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -6,10 +6,10 @@
  */
 
 // Wolf includes
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/frame_base.h"
-#include "base/trajectory_base.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/frame/frame_base.h"
+#include "base/trajectory/trajectory_base.h"
 
 // STL includes
 #include <list>
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
index 5885553ed2fbbed5b6769d1f361c84cb9195509f..0fabac44e3252fafad9e940e9bcfd1b62b0e8ec0 100644
--- a/src/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -5,9 +5,9 @@
  *      \author: jsola
  */
 
-#include "base/frame_base.h"
-#include "base/state_quaternion.h"
-#include "base/time_stamp.h"
+#include "base/frame/frame_base.h"
+#include "base/state_block/state_quaternion.h"
+#include "base/common/time_stamp.h"
 
 #include <iostream>
 
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index b35d0dc1b335d88759ecdcd3b4c277a309b58636..b608bb9583320b361d88ea2a8c48ff973e85c08d 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -7,7 +7,7 @@
 
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_GPS_fix.h"
-#include "base/hardware_base.h"
+#include "base/hardware/hardware_base.h"
 #include "base/sensor/sensor_camera.h"
 #include "base/sensor/sensor_odom_2D.h"
 #include "../sensor_imu.h"
@@ -17,9 +17,9 @@
 #include "base/processor/processor_odom_3D.h"
 #include "../processor_image_feature.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 #include <iostream>
 #include <iomanip>
diff --git a/src/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp
index b516acd098a93ce8c060e0c380555ec29041be3a..3b7bdfab70281f10e27b17891564a9b633ac92e1 100644
--- a/src/examples/test_wolf_logging.cpp
+++ b/src/examples/test_wolf_logging.cpp
@@ -5,8 +5,8 @@
  * \author: Jeremie Deray
  */
 
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
 
 int main(int, char*[])
 {
diff --git a/src/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp
index a2e3ccef5d5534daf5be8b1a360e2c30de6b3e6d..ff78c97b0866ab66440ba91d20d8d0db503aed26 100644
--- a/src/examples/test_wolf_root.cpp
+++ b/src/examples/test_wolf_root.cpp
@@ -6,7 +6,7 @@
  */
 
 //Wolf
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 //std
 #include <iostream>
diff --git a/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
index cfd03d18d073cb9bb85ccc93f342e3ef9c86f52a..5181d73acd049da2469ae0cf6a61296317b865df 100644
--- a/src/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -5,10 +5,10 @@
  *      \author: jsola
  */
 
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
 #include "yaml/yaml_conversion.h"
 #include "processor_image_feature.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 #include <yaml-cpp/yaml.h>
 
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index c86bec4aa9b35d7e7879f5a6c7f53c9181e78570..7ad85937a809b9d524d2f5c30493f1c17d3a5102 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -1,5 +1,5 @@
 #include "base/factor/factor_analytic.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 namespace wolf {
 
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index e3dc7b1ab767657d613dc99c9f41925cf6a290da..188e873b654ec18007b161badd29d5c72fdd8194 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -1,5 +1,5 @@
 #include "base/factor/factor_base.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 #include "base/landmark/landmark_base.h"
 
 namespace wolf {
diff --git a/src/frame_base.cpp b/src/frame/frame_base.cpp
similarity index 95%
rename from src/frame_base.cpp
rename to src/frame/frame_base.cpp
index ccd51dc7f71cf6e696ed68c5e77a50f314b5bf45..cb7f089c1f21adad761f5fd7c13f0eee786b6741 100644
--- a/src/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -1,11 +1,11 @@
 
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 #include "base/factor/factor_base.h"
-#include "base/trajectory_base.h"
+#include "base/trajectory/trajectory_base.h"
 #include "base/capture/capture_base.h"
-#include "base/state_block.h"
-#include "base/state_angle.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_angle.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf {
 
@@ -177,11 +177,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
 
 Eigen::VectorXs FrameBase::getState() const
 {
-    SizeEigen size = 0;
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb)
-            size += sb->getSize();
-    Eigen::VectorXs state(size);
+    Eigen::VectorXs state;
 
     getState(state);
 
@@ -195,10 +191,9 @@ void FrameBase::getState(Eigen::VectorXs& _state) const
         if (sb)
             size += sb->getSize();
 
-    assert(_state.size() == size && "Wrong state vector size");
+    _state = Eigen::VectorXs(size);
 
     SizeEigen index = 0;
-
     for (StateBlockPtr sb : state_block_vec_)
         if (sb)
         {
@@ -221,11 +216,6 @@ bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
     return getProblem()->getFrameCovariance(shared_from_this(), _cov);
 }
 
-Eigen::MatrixXs FrameBase::getCovariance() const
-{
-    return getProblem()->getFrameCovariance(shared_from_this());
-}
-
 FrameBasePtr FrameBase::getPreviousFrame() const
 {
     //std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
@@ -384,7 +374,7 @@ FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
 
 } // namespace wolf
 
-#include "base/factory.h"
+#include "base/common/factory.h"
 namespace wolf
 {
 namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered  = FrameFactory::get().registerCreator("PO 2D",  FrameBase::create_PO_2D ); }
diff --git a/src/hardware_base.cpp b/src/hardware/hardware_base.cpp
similarity index 93%
rename from src/hardware_base.cpp
rename to src/hardware/hardware_base.cpp
index 945b412c87cbab6be87a8d7d5536d2850996a14f..69b23c34c1c46a6f79f38dba594544b68cd9c8b3 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -1,4 +1,4 @@
-#include "base/hardware_base.h"
+#include "base/hardware/hardware_base.h"
 #include "base/sensor/sensor_base.h"
 
 namespace wolf {
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 1daa37470b0c1ce87d47e4d174344fd09e471d9d..4fde7a42c99df3a63bcb9ceda382ae970d3a03b3 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -1,7 +1,7 @@
 #include "base/landmark/landmark_AHP.h"
 
-#include "base/state_homogeneous_3D.h"
-#include "base/factory.h"
+#include "base/state_block/state_homogeneous_3D.h"
+#include "base/common/factory.h"
 #include "base/yaml/yaml_conversion.h"
 
 namespace wolf {
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index d1fb63043ac989955ab8954c9cc9a19754184bec..4b3e4a8a29456b4313c6ac4a9853995d94ea6357 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -1,8 +1,8 @@
 
 #include "base/landmark/landmark_base.h"
 #include "base/factor/factor_base.h"
-#include "base/map_base.h"
-#include "base/state_block.h"
+#include "base/map/map_base.h"
+#include "base/state_block/state_block.h"
 #include "base/yaml/yaml_conversion.h"
 
 namespace wolf {
@@ -94,11 +94,6 @@ void LandmarkBase::registerNewStateBlocks()
     }
 }
 
-Eigen::MatrixXs LandmarkBase::getCovariance() const
-{
-    return getProblem()->getLandmarkCovariance(shared_from_this());
-}
-
 bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
 {
     return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
@@ -122,11 +117,7 @@ void LandmarkBase::removeStateBlocks()
 
 Eigen::VectorXs LandmarkBase::getState() const
 {
-    SizeEigen size = 0;
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb)
-            size += sb->getSize();
-    Eigen::VectorXs state(size);
+    Eigen::VectorXs state;
 
     getState(state);
 
@@ -140,10 +131,9 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const
         if (sb)
             size += sb->getSize();
 
-    assert(_state.size() == size && "Wrong state vector size");
+    _state = Eigen::VectorXs(size);
 
     SizeEigen index = 0;
-
     for (StateBlockPtr sb : state_block_vec_)
         if (sb)
         {
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
index 3a424869ba5a8775bda6025b7a83aeadda89009a..3a043fd7d61efda2b2927a9335c6ac92d20dfb32 100644
--- a/src/landmark/landmark_container.cpp
+++ b/src/landmark/landmark_container.cpp
@@ -1,6 +1,6 @@
 
 #include "base/landmark/landmark_container.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 namespace wolf {
 
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index d7c5c7d76a3e3b3237805b0b55218f9c258cbeaf..bbe0747e01b61e173c71aeaba16b0881ae6175a7 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -7,11 +7,11 @@
 
 #include "base/feature/feature_polyline_2D.h"
 #include "base/landmark/landmark_polyline_2D.h"
-#include "base/local_parametrization_polyline_extreme.h"
+#include "base/state_block/local_parametrization_polyline_extreme.h"
 #include "base/factor/factor_point_2D.h"
 #include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/factory.h"
+#include "base/state_block/state_block.h"
+#include "base/common/factory.h"
 #include "base/yaml/yaml_conversion.h"
 
 namespace wolf
diff --git a/src/map_base.cpp b/src/map/map_base.cpp
similarity index 97%
rename from src/map_base.cpp
rename to src/map/map_base.cpp
index d1ad03121691b64052bfab01372010f68de863f5..d7d7a1dada21321560b0320bbc1f452a66ce5b7e 100644
--- a/src/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -1,8 +1,8 @@
 
 // wolf
-#include "base/map_base.h"
+#include "base/map/map_base.h"
 #include "base/landmark/landmark_base.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>
diff --git a/src/problem.cpp b/src/problem/problem.cpp
similarity index 98%
rename from src/problem.cpp
rename to src/problem/problem.cpp
index 8e682083d4ab8204c38c5d0f874ba5f60c39d78c..301eb1a6d64cfd98371c9d7d6f0c098f9a3f87c7 100644
--- a/src/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1,8 +1,8 @@
 // wolf includes
-#include "base/problem.h"
-#include "base/hardware_base.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/problem/problem.h"
+#include "base/hardware/hardware_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
 #include "base/sensor/sensor_base.h"
 #include "base/processor/processor_motion.h"
 #include "base/processor/processor_tracker.h"
@@ -10,7 +10,7 @@
 #include "base/factor/factor_base.h"
 #include "base/sensor/sensor_factory.h"
 #include "base/processor/processor_factory.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 
 // IRI libs includes
@@ -279,8 +279,6 @@ Eigen::VectorXs Problem::getCurrentState()
 
 void Problem::getCurrentState(Eigen::VectorXs& state)
 {
-    assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
     if (processor_motion_ptr_ != nullptr)
         processor_motion_ptr_->getCurrentState(state);
     else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
@@ -291,8 +289,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
 
 void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
 {
-    assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
     if (processor_motion_ptr_ != nullptr)
     {
         processor_motion_ptr_->getCurrentState(state);
@@ -309,8 +305,6 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
 
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
 {
-    assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size");
-
     // try to get the state from processor_motion if any, otherwise...
     if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
     {
@@ -594,6 +588,16 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
 
     const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
 
+    // computing size
+    SizeEigen sz = 0;
+    for (const auto& sb : state_bloc_vec)
+        if (sb)
+            sz += sb->getSize();
+
+    // resizing
+    _covariance = Eigen::MatrixXs(sz, sz);
+
+    // filling covariance
     for (const auto& sb_i : state_bloc_vec)
     {
         if (sb_i)
@@ -613,23 +617,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
     return success;
 }
 
-Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr)
-{
-    SizeEigen sz = 0;
-    for (const auto& sb : _frame_ptr->getStateBlockVec())
-        if (sb)
-            sz += sb->getSize();
-
-    Eigen::MatrixXs covariance(sz, sz);
-
-    getFrameCovariance(_frame_ptr, covariance);
-    return covariance;
-}
-
-Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
+bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
 {
     FrameBasePtr frm = getLastKeyFrame();
-    return getFrameCovariance(frm);
+    return getFrameCovariance(frm, cov);
 }
 
 bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
@@ -639,6 +630,17 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
 
     const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
 
+    // computing size
+    SizeEigen sz = 0;
+    for (const auto& sb : state_bloc_vec)
+        if (sb)
+            sz += sb->getSize();
+
+    // resizing
+    _covariance = Eigen::MatrixXs(sz, sz);
+
+    // filling covariance
+
     for (const auto& sb_i : state_bloc_vec)
     {
         if (sb_i)
@@ -658,19 +660,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
     return success;
 }
 
-Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr)
-{
-    SizeEigen sz = 0;
-    for (const auto& sb : _landmark_ptr->getStateBlockVec())
-        if (sb)
-            sz += sb->getSize();
-
-    Eigen::MatrixXs covariance(sz, sz);
-
-    getLandmarkCovariance(_landmark_ptr, covariance);
-    return covariance;
-}
-
 MapBasePtr Problem::getMap()
 {
     return map_ptr_;
diff --git a/src/motion_buffer.cpp b/src/processor/motion_buffer.cpp
similarity index 99%
rename from src/motion_buffer.cpp
rename to src/processor/motion_buffer.cpp
index 20d4fc26060fbd12087ae51b256ca85fe528de2d..2828ad4d0262f77af8fe09a45bc28f93a83ff7b7 100644
--- a/src/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -1,4 +1,4 @@
-#include "base/motion_buffer.h"
+#include "base/processor/motion_buffer.h"
 namespace wolf
 {
 
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index 3f1f5b2445ea42bda8b863beddaf52210bbf4c4c..2dfa2cd7de6b14c3aea08eab02782d30235c15df 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -1,7 +1,7 @@
 // wolf
 #include "base/processor/processor_IMU.h"
 #include "base/factor/factor_IMU.h"
-#include "base/IMU_tools.h"
+#include "base/math/IMU_tools.h"
 
 namespace wolf {
 
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 82fa41c137c0211bb78a699125d287118edc1775..0679909164d86a96ac609a930619f35ff6a97fe3 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -1,7 +1,7 @@
 #include "base/processor/processor_base.h"
 #include "base/processor/processor_motion.h"
 #include "base/capture/capture_base.h"
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 
 namespace wolf {
 
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 4e307962e94a3fc2e205cf7cd1c80f480d2871c4..881ebedec5cc8988980a41070a71dbbd50e24df8 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -5,7 +5,7 @@
 #include "base/capture/capture_wheel_joint_position.h"
 #include "base/capture/capture_velocity.h"
 
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 #include "base/factor/factor_odom_2D.h"
 #include "base/feature/feature_diff_drive.h"
 
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 9db14170b38e556224ea7a101a33e0db9e83746c..2ecd9b22ec00985f94a827ad6aa8447d57a0ade0 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -199,8 +199,12 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& f
                                                            Eigen::Vector5s& ellipse)
 {
   // get 3x3 covariance matrix AKA variance
-  const Eigen::MatrixXs covariance =
-      getProblem()->getFrameCovariance(frame_ptr);
+  Eigen::MatrixXs covariance;
+  if (!getProblem()->getFrameCovariance(frame_ptr, covariance))
+  {
+      WOLF_WARN("Frame covariance not found!");
+      return false;
+  }
 
   if (!isCovariance(covariance))
   {
@@ -244,9 +248,18 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr&
   ellipsoid(2) = frame_position(2);
 
   // get 9x9 covariance matrix AKA variance
-  const Eigen::MatrixXs covariance = getProblem()->getFrameCovariance(frame_pointer);
+  Eigen::MatrixXs covariance;
+  if (!getProblem()->getFrameCovariance(frame_pointer, covariance))
+  {
+      WOLF_WARN("Frame covariance not found!");
+      return false;
+  }
 
-  if (!isCovariance(covariance)) return false;
+  if (!isCovariance(covariance))
+  {
+      WOLF_WARN("Covariance is not proper !");
+      return false;
+  }
 
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
   const Scalar eigenvalue1 = solver.eigenvalues()[0];     // smaller value
@@ -463,10 +476,11 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
   query_pose << query->getP()->getState(),
                 query->getO()->getState();
 
-  const Eigen::MatrixXs traj_covariance  = getProblem()->getFrameCovariance(trajectory);
-  const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query);
-
-  if ( !isCovariance(traj_covariance) || !isCovariance(query_covariance))
+  Eigen::MatrixXs traj_covariance, query_covariance;
+  if ( !getProblem()->getFrameCovariance(trajectory, traj_covariance) ||
+       !getProblem()->getFrameCovariance(query, query_covariance) ||
+       !isCovariance(traj_covariance) ||
+       !isCovariance(query_covariance))
     return distance;
 
   const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 0ee10e6f75b5fa6e843421eaba7728edc7b2f4ea..793b279883beaa3411cedc781e128da718fcc178 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -21,7 +21,7 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
 {
 }
 
-unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
+unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
 {
     /* Rationale: A keyFrame will be created using the last Capture.
      * First, we work on the last Capture to detect new Features,
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 4414d8caf499e187e28961b54f8a65e80455716a..9218dba63738e88a65e2fcf3572ed2a1b5757237 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -107,10 +107,13 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
 }
 
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
     // in corners_last_ remain all not tracked corners
     _features_incoming_out = std::move(corners_last_);
+    if (_max_features != -1 && _features_incoming_out.size() > _max_features)
+        _features_incoming_out.resize(_max_features);
+
     return _features_incoming_out.size();
 }
 
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index 060b54e83a3aba8e7b483c2e2678b88b7e468678..b5b225f8490b8cf4b4f6823be870fc3f22ab8df7 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -49,12 +49,19 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
+    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 = 1; i <= _max_features; i++)
+    for (unsigned int i = 0; i < max_features; i++)
     {
         n_feature_++;
         FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 3f6bec8fe828580d8b04e5b0c390a8441cc24a9e..c2f68524b6fcae33ff95a838601618a613784ff9 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
     }
 }
 
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -252,7 +252,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
     cv::KeyPointsFilter keypoint_filter;
     unsigned int n_new_features = 0;
 
-    for (unsigned int n_iterations = 0; _max_new_features == 0 || n_iterations < _max_new_features; n_iterations++)
+    for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; n_iterations++)
     {
 
         if (active_search_ptr_->pickEmptyRoi(roi))
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 980c652af00ecb490250ac0baabb375ddf5ba98e..620cb4c05988e4552232c78b14d3a8b6bab072ee 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -134,7 +134,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
 }
 
 
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -143,7 +143,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
 //    WOLF_TRACE("======== DetectNewFeatures =========");
 //    // ===========================================
 
-    for (unsigned int n_iterations = 0; n_iterations < _max_new_features; ++n_iterations)
+    for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; ++n_iterations)
     {
         Eigen::Vector2i cell_last;
         if (capture_last_->grid_features_->pickEmptyTrackingCell(cell_last))
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index d19610391edb98ee2565c2f477970a11c0088075..c03d8712f2f48b7eef7f17e08f7fdfa7a9a0045c 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -6,7 +6,7 @@
  */
 
 #include "base/processor/processor_tracker_landmark.h"
-#include "base/map_base.h"
+#include "base/map/map_base.h"
 
 #include <utility>
 
@@ -60,7 +60,7 @@ void ProcessorTrackerLandmark::resetDerived()
     //            std::cout << "\t" << match.first->id() << " to " << match.second.landmark_ptr_->id() << std::endl;
 }
 
-unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_features)
+unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
 {
     /* Rationale: A keyFrame will be created using the last Capture.
      * First, we work on this Capture to detect new Features,
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index aeba30f8e908a72a57aad5b5f617edfa249b8982..884f867c7fe46c0bac1d7539c4dde58ddbf02a72 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -1,5 +1,5 @@
 #include "base/processor/processor_tracker_landmark_corner.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 namespace wolf
 {
@@ -375,10 +375,13 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
                                               _feature_ptr->getMeasurement()(3));
 }
 
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
-    // already computed since each scan is computed in preprocess()
+    // in corners_last_ remain all not tracked corners, already computed in preprocess()
     _features_incoming_out = std::move(corners_last_);
+    if (_max_features != -1 && _features_incoming_out.size() > _max_features)
+        _features_incoming_out.resize(_max_features);
+
     return _features_incoming_out.size();
 }
 
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index b2f92d72dd8bdc8d377905977185da6ae776688f..f514cb7efd76a571c7cd6f74c1353b21ca745050 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -63,16 +63,25 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < 4;
 }
 
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
 
-    // detecting 5 new features
-    for (unsigned int i = 1; i <= _max_features; i++)
+    unsigned int max_features = _max_features;
+
+    if (max_features == -1)
+    {
+        max_features = 10;
+        WOLF_INFO("max_features unlimited, setting it to " , max_features);
+    }
+
+    // detecting new features
+    for (unsigned int i = 1; i <= max_features; i++)
     {
         n_feature_++;
-        _features_incoming_out.push_back(
-                std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
+        _features_incoming_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE",
+                                                                       n_feature_ * Eigen::Vector1s::Ones(),
+                                                                       Eigen::MatrixXs::Ones(1, 1)));
         std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
     }
     return _features_incoming_out.size();
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index a88ec47c932f3c1c7f9098a532285c20940dbeda..515daff2acbd60b429b5ae35af570171861db9ac 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -4,14 +4,14 @@
 #include "base/factor/factor_AHP.h"
 #include "base/feature/feature_base.h"
 #include "base/feature/feature_point_image.h"
-#include "base/frame_base.h"
-#include "base/logging.h"
-#include "base/map_base.h"
-#include "base/pinhole_tools.h"
-#include "base/problem.h"
+#include "base/frame/frame_base.h"
+#include "base/utils/logging.h"
+#include "base/map/map_base.h"
+#include "base/math/pinhole_tools.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_camera.h"
-#include "base/state_block.h"
-#include "base/time_stamp.h"
+#include "base/state_block/state_block.h"
+#include "base/common/time_stamp.h"
 
 // vision_utils
 #include <detectors.h>
@@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
 //    return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -233,7 +233,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int
     cv::KeyPointsFilter keypoint_filter;
     unsigned int n_new_features = 0;
 
-    for (unsigned int n_iterations = 0; n_iterations < _max_features; n_iterations++)
+    for (unsigned int n_iterations = 0; _max_features == -1 || n_iterations < _max_features; n_iterations++)
     {
         if (active_search_ptr_->pickEmptyRoi(roi))
         {
diff --git a/src/track_matrix.cpp b/src/processor/track_matrix.cpp
similarity index 99%
rename from src/track_matrix.cpp
rename to src/processor/track_matrix.cpp
index 50ab808912128f28279b9d0ad39b8580f251faca..8db44ce4abb4e0d3dc19e5ec3f53403ee642caf3 100644
--- a/src/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "base/track_matrix.h"
+#include "base/processor/track_matrix.h"
 
 namespace wolf
 {
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
index 323616be956041905389ad5b083c85c11af3707e..f6c1eace8819cd0cbf16b3fa568eddfec1ddde4b 100644
--- a/src/sensor/sensor_GPS.cpp
+++ b/src/sensor/sensor_GPS.cpp
@@ -1,7 +1,7 @@
 
 #include "base/sensor/sensor_GPS.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf {
 
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index c69f99b6a2254b277dc250fed85b3f2105159db4..3c781095f36faffe087c4f98426497bbbe37bb69 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/sensor_GPS_fix.cpp
@@ -1,6 +1,6 @@
 #include "base/sensor/sensor_GPS_fix.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf {
 
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index f97edf645e607b08534ca421db1bd09070bf8d70..d5b6841636a9d9a06d1cebb5a81aecd8cb455647 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -1,6 +1,6 @@
 #include "base/sensor/sensor_IMU.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf {
 
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index a30f777fc109da531c0d56237a650ccad0090bf5..209dc12e04ed770ec797f2d10b27173b17fe0458 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,6 +1,6 @@
 #include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "base/factor/factor_block_absolute.h"
 #include "base/factor/factor_quaternion_absolute.h"
 
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 54c08c69f3a263b6d71a1b461baa393e62db0d10..cc9615cf61ebdae48f4dc04015a29f20de7cd2ce 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -1,8 +1,8 @@
 #include "base/sensor/sensor_camera.h"
 
-#include "base/pinhole_tools.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/math/pinhole_tools.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf
 {
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index d5fdb9e5ee702a8789307eb875e8d026a9d608b3..43894b7d6aded5d57016666b949bd1538e296355 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -1,7 +1,7 @@
 #include "base/sensor/sensor_diff_drive.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/capture/capture_motion.h"
-#include "base/eigen_assert.h"
+#include "base/utils/eigen_assert.h"
 
 namespace wolf {
 
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index a7a5677af497ccb2f925dce9ae993e160b860fd5..5c8b72ae331b714bbe33ebf64b1de14cb2cf8c0e 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -1,5 +1,5 @@
 #include "base/sensor/sensor_laser_2D.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 
 namespace wolf {
 
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index 5b881b83f2fbfe5f4c84321a46fd3fb3b8618a76..0fba6c4b5660fa08c28e14c69a001b7538db9237 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
 #include "base/sensor/sensor_odom_2D.h"
-#include "base/state_block.h"
-#include "base/state_angle.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_angle.h"
 
 namespace wolf {
 
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index 0a5c2d7961b5d36e171e6b9d29fafdc30580a74e..5a3736d5e7e111ffbe20359b29c0d562f71a35e4 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3D.cpp
@@ -7,8 +7,8 @@
 
 #include "base/sensor/sensor_odom_3D.h"
 
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 
 namespace wolf {
 
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 493bc9dcd60683a2836811175a6c57c13c63ff98..adc0dabd9d9ae889b95ba2b2aaf33b7237094d3a 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -1,6 +1,6 @@
 #include "base/solver/solver_manager.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/map/map_base.h"
 #include "base/landmark/landmark_base.h"
 
 namespace wolf {
diff --git a/src/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
similarity index 88%
rename from src/local_parametrization_base.cpp
rename to src/state_block/local_parametrization_base.cpp
index e2bc1cf4e36aafd25c8209826d3c8afe43828d62..485b3171b0a1d94115a428aa219effe9460f1b10 100644
--- a/src/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -1,4 +1,4 @@
-#include "base/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/src/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
similarity index 93%
rename from src/local_parametrization_homogeneous.cpp
rename to src/state_block/local_parametrization_homogeneous.cpp
index 14abaecd1a9fd93cfd1da4b9a6e1e1d0f9982dbd..b8982f89083c23207b0ae3032d1fd103c6c7b6ac 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -5,9 +5,9 @@
  *      Author: jsola
  */
 
-#include "base/local_parametrization_homogeneous.h"
+#include "base/state_block/local_parametrization_homogeneous.h"
 #include "iostream"
-#include "base/rotations.h" // we use quaternion algebra here
+#include "base/math/rotations.h" // we use quaternion algebra here
 
 namespace wolf {
 
diff --git a/src/local_parametrization_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp
similarity index 93%
rename from src/local_parametrization_polyline_extreme.cpp
rename to src/state_block/local_parametrization_polyline_extreme.cpp
index da318cafb9e723aa1fecfe9f19d6a3195df7fde8..803c85b4f612b4dcfe9306fe54b2105e1be05506 100644
--- a/src/local_parametrization_polyline_extreme.cpp
+++ b/src/state_block/local_parametrization_polyline_extreme.cpp
@@ -1,6 +1,6 @@
-#include "base/local_parametrization_polyline_extreme.h"
-#include "base/state_block.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_polyline_extreme.h"
+#include "base/state_block/state_block.h"
+#include "base/math/rotations.h"
 
 namespace wolf {
 
diff --git a/src/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp
similarity index 97%
rename from src/local_parametrization_quaternion.cpp
rename to src/state_block/local_parametrization_quaternion.cpp
index d2af8f544f21f8f83528c7db6371f0a8407f12d0..1c2655f883ed00474d84feda1c2bbe47bc42e1aa 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/state_block/local_parametrization_quaternion.cpp
@@ -1,6 +1,6 @@
 
-#include "base/local_parametrization_quaternion.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_quaternion.h"
+#include "base/math/rotations.h"
 
 #include <iostream>
 namespace wolf {
diff --git a/src/state_block.cpp b/src/state_block/state_block.cpp
similarity index 94%
rename from src/state_block.cpp
rename to src/state_block/state_block.cpp
index b4427c2aae025a794ae433bcb723011f1110f499..96df91772539470b66ed81887d68dc72b19c1def 100644
--- a/src/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -1,4 +1,4 @@
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 namespace wolf
 {
 
diff --git a/src/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
similarity index 97%
rename from src/trajectory_base.cpp
rename to src/trajectory/trajectory_base.cpp
index 5820b99ba5df29f281ee2e4e61c82368a93d3561..086afab738a5b66cebdde25cdda862606d28b924 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -1,5 +1,5 @@
-#include "base/trajectory_base.h"
-#include "base/frame_base.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/frame/frame_base.h"
 
 namespace wolf {
 
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 2cfc70f46321ea3af69d657b47540258b4b17ebd..5b3f12ac6100f079d3653acea7f941d51afdca2d 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -10,7 +10,7 @@
 #include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index a02d44208d414294d203661a27da019b94d04afa..47e0a32e2aacad260db22b621341352a825800df 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -9,7 +9,7 @@
 #include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index 5c2172ea9f26c3561f7fc48c5c56d4be7695ed1c..c63e3d9b5c50e3add064bc334109be163ac578d5 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -10,7 +10,7 @@
 
 // wolf
 #include "base/processor/processor_odom_3D.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index a3f9362b5a91d32d793a6459e409c2849f151bc4..0af63b58212e96fe16fb0e52100696e0c1a2fe15 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -10,7 +10,7 @@
 #include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index cc3dbb649b7d2c14c92ec4e78f2d43c0c343b254..79cf183cdad989a3a123488b10f878c86fb142d3 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -10,7 +10,7 @@
 #include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 5e5e3df19cd91a8735f6625ecd2d551deb7e7b77..2b4bfc4d865d01ede338ae2aaf45ef0ffe21838a 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -10,7 +10,7 @@
 
 // wolf
 #include "base/sensor/sensor_camera.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index e687c14463b746d147d791338afc5e34120fe8f3..bd553d20dd3145f8d899710094d0a4d7e98310cd 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -10,7 +10,7 @@
 
 // wolf
 //#include "base/intrinsics_factory.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
 #include "base/sensor/sensor_laser_2D.h"
 
 // yaml library
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index 63a7baa2c4cad96e3ca297950df76611fb4ed7ba..d4e90dd831e5bac30eeb680f8e114b27e412d12d 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -10,7 +10,7 @@
 
 // wolf
 #include "base/sensor/sensor_odom_3D.h"
-#include "base/factory.h"
+#include "base/common/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index 7487eb2fe49ada12c03120ce83810abf3d0d8943..d2657e4156cf6991d3342505cb558712837cb344 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -8,13 +8,13 @@
 //Wolf
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/processor/processor_odom_3D.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
 // make my life easier
 using namespace Eigen;
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
index e41007154f27c8c6de6845db934215df22d7c382..3cc7d33e28f8149d33ee01d109357569e3dc44c5 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_IMU_tools.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "base/IMU_tools.h"
+#include "base/math/IMU_tools.h"
 #include "utils_gtest.h"
 
 using namespace Eigen;
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index d6e3b9b1c2a2154677f4a2a77094b4bac93146f0..002aa206ba306ad9cd84e978bb8f0a6700653eb9 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -6,7 +6,7 @@
  */
 
 
-#include "base/SE3.h"
+#include "base/math/SE3.h"
 #include "utils_gtest.h"
 
 
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 71217c8e6df3dc2cd0a9ce5bc2e73c97ec19c72a..3d6bcb3aecb338bba08324ff049f1d226fa4d406 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -8,7 +8,7 @@
 #include "utils_gtest.h"
 
 #include "base/capture/capture_base.h"
-#include "base/state_angle.h"
+#include "base/state_block/state_angle.h"
 
 using namespace wolf;
 using namespace Eigen;
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index cbe91a6fdf1c276f55bf50e9c32451f0618feb5b..78492ae19bdc78f500f50830ea093bcc4f16c35f 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -6,18 +6,18 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/capture/capture_void.h"
 #include "base/factor/factor_pose_2D.h"
 #include "base/factor/factor_quaternion_absolute.h"
 #include "base/solver/solver_manager.h"
 #include "base/ceres_wrapper/ceres_manager.h"
-#include "base/local_parametrization_angle.h"
-#include "base/local_parametrization_quaternion.h"
+#include "base/state_block/local_parametrization_angle.h"
+#include "base/state_block/local_parametrization_quaternion.h"
 
 #include "ceres/ceres.h"
 
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index 649ba85e9b5477d38498b3f80678eaf25e87697b..b3c3010491be08806da65cb6a5e6d953f23943cf 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,6 +1,6 @@
 #include "utils_gtest.h"
 
-#include "base/eigen_predicates.h"
+#include "base/utils/eigen_predicates.h"
 
 TEST(TestEigenPredicates, TestEigenDynPredZero)
 {
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index 6c8990c291af5bb43dc9e042f9ca60dc84854e75..e9692a62c92802d6fe234eae49ac0452a6b86d99 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -14,7 +14,7 @@
 #include "ceres_wrapper/ceres_manager.h"
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
 #include <iostream>
 #include <fstream>
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 7559bfa7582aa1449232391806e498fb294d03be..21c7c912f801b39adcf3b5c75e56ca03d08e83c2 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -6,10 +6,10 @@
  */
 
 #include "base/factor/factor_autodiff_distance_3D.h"
-#include "base/problem.h"
-#include "base/logging.h"
+#include "base/problem/problem.h"
+#include "base/utils/logging.h"
 #include "base/ceres_wrapper/ceres_manager.h"
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 
 #include "utils_gtest.h"
 
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 71401381fe35c768d047b8324e0b2e2f74df35c4..98af13f6e834f73bd4bbab7e524cf8928d0467af 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -1,6 +1,6 @@
 #include "utils_gtest.h"
 
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
 #include "base/ceres_wrapper/ceres_manager.h"
 #include "base/processor/processor_tracker_feature_trifocal.h"
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 82828c6c274a3ed2e72d53f15e8f625583644697..082e438d0b799e1ed32075f28e6f61b7450da8e0 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -2,12 +2,12 @@
 #include "base/capture/capture_IMU.h"
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
 class FeatureIMU_test : public testing::Test
 {
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 06436282a934f4b61bcfe04e50c77b1f3f27eea8..9b4c7b6cf882b1f8104fa71ac3d60e12d8580137 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -6,9 +6,9 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/frame_base.h"
+#include "base/frame/frame_base.h"
 #include "base/sensor/sensor_odom_2D.h"
 #include "base/processor/processor_odom_2D.h"
 #include "base/factor/factor_odom_2D.h"
@@ -129,7 +129,7 @@ TEST(FrameBase, LinksToTree)
     ASSERT_TRUE(F1->getCaptureList().empty());
 }
 
-#include "base/state_quaternion.h"
+#include "base/state_block/state_quaternion.h"
 TEST(FrameBase, GetSetState)
 {
     // Create PQV_3D state blocks
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 42e01489ed722497bdaad1d818ae572e1d2c0100..45d570671bbe43e6c51d8a186ed1a5cc081550ba 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -6,13 +6,13 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/local_parametrization_quaternion.h"
-#include "base/local_parametrization_homogeneous.h"
-#include "base/rotations.h"
+#include "base/state_block/local_parametrization_quaternion.h"
+#include "base/state_block/local_parametrization_homogeneous.h"
+#include "base/math/rotations.h"
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 #include <iostream>
 
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 1dfcce9353f5726f5e69593f99e8a7dc3782cffd..06f9fd20e97dc4bb64033da673e9e04c29bc277c 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 using namespace Eigen;
 using namespace wolf;
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 6d0301692fc5bea5b444a6aa582e8170d489294c..6aee437dc13357de1618f036d27a90dfc094acd5 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -6,11 +6,11 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/motion_buffer.h"
+#include "base/processor/motion_buffer.h"
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 #include <iostream>
 
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 5f479b19ec09a04c604aaca010c98cfd6bd37c6f..2aa224b3143b723ec7d98be9183c26392522206a 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -13,8 +13,8 @@
 
 // Wolf includes
 #include "base/sensor/sensor_odom_2D.h"
-#include "base/state_block.h"
-#include "base/wolf.h"
+#include "base/state_block/state_block.h"
+#include "base/common/wolf.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
 // STL includes
@@ -92,7 +92,9 @@ void show(const ProblemPtr& problem)
                 }
             }
             cout << "  state: \n" << F->getState().transpose() << endl;
-            cout << "  covariance: \n" << problem->getFrameCovariance(F) << endl;
+            Eigen::MatrixXs cov;
+            problem->getFrameCovariance(F,cov);
+            cout << "  covariance: \n" << cov << endl;
         }
     }
 }
@@ -163,12 +165,18 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
           0,    1.91, 0.48,
           0,    0.48, 0.14;
 
+    // get covariances
+    MatrixXs P0_solver, P1_solver, P2_solver;
+    ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver));
+    ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
+    ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
+
     ASSERT_POSE2D_APPROX(F0->getState(), Vector3s(0,0,0), 1e-6);
-    ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F0), P0, 1e-6);
+    ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
     ASSERT_POSE2D_APPROX(F1->getState(), Vector3s(2,0,0), 1e-6);
-    ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F1), P1, 1e-6);
+    ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
     ASSERT_POSE2D_APPROX(F2->getState(), Vector3s(4,0,0), 1e-6);
-    ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F2), P2, 1e-6);
+    ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
 }
 
 TEST(Odom2D, VoteForKfAndSolve)
@@ -285,7 +293,9 @@ TEST(Odom2D, VoteForKfAndSolve)
 //    std::cout << report << std::endl;
     ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
-    ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector[5], 1e-6);
+    MatrixXs computed_cov;
+    ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+    ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6);
 }
 
 TEST(Odom2D, KF_callback)
@@ -413,7 +423,9 @@ TEST(Odom2D, KF_callback)
     ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
     ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
-    ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance()      , integrated_cov_vector [n_split], 1e-6);
+    MatrixXs computed_cov;
+    ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+    ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
 
     ////////////////////////////////////////////////////////////////
     // Split between keyframes, exact timestamp
@@ -449,12 +461,16 @@ TEST(Odom2D, KF_callback)
     problem->print(4,1,1,1);
 
     // check the split KF
-    ASSERT_POSE2D_APPROX(keyframe_1->getState()                  , integrated_pose_vector[m_split], 1e-6);
-    ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6);
+    MatrixXs KF1_cov;
+    ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
+    ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_cov,                integrated_cov_vector [m_split], 1e-6);
 
     // check other KF in the future of the split KF
-    ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
-    ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_2)   , integrated_cov_vector [n_split], 1e-6);
+    MatrixXs KF2_cov;
+    ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
+    ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_cov,                                integrated_cov_vector [n_split], 1e-6);
 
     // Check full trajectory
     t = t0;
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index ab4f82bb186b949e5dd0d71b5e9e812972a6708e..34cb26dc38598ad2860c765b984fa891ff8e95c4 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -7,8 +7,8 @@
 
 #include "utils_gtest.h"
 
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
 
 #include "base/processor/processor_odom_3D.h"
 
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 8f68a5bd532c5a1408d0ba9654ec9f27283c7ba3..d4ed20575f2ae0920b992be6f7116abc1e2fc439 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -13,7 +13,7 @@
 #include "base/processor/processor_tracker_feature_dummy.h"
 #include "base/capture/capture_void.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 
 // STL
 #include <iterator>
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index e39c870881bf6e2abb57e771a2b79a3ed3706ce2..79f7aff496e524368e17ee087fed609efb2dd721 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -6,9 +6,9 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 #include "base/sensor/sensor_odom_3D.h"
 
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
index fb36c6d127b11d14b86c8887fa32aa1b23d07391..378757f09922e7ae48cf16b96a41b497cb7e3717 100644
--- a/test/gtest_pinhole.cpp
+++ b/test/gtest_pinhole.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "base/pinhole_tools.h"
+#include "base/math/pinhole_tools.h"
 #include "utils_gtest.h"
 
 using namespace Eigen;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 8ee7623dc68b7f637677fd24ceb6f90b083a4b39..2c5102882ac6ee6c9bd25aeb46c847df56b7162a 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -6,9 +6,9 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_base.h"
 #include "base/sensor/sensor_odom_3D.h"
 #include "base/processor/processor_odom_3D.h"
@@ -262,11 +262,20 @@ TEST(Problem, Covariances)
     St->unfixExtrinsics();
     FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
 
-    Eigen::MatrixXs Cov = P->getFrameCovariance(F);
+    // set covariance (they are not computed without a solver)
+    P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
+    P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity());
+    P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero());
+
+    // get covariance
+    Eigen::MatrixXs Cov;
+    ASSERT_TRUE(P->getFrameCovariance(F, Cov));
 
     // FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations)
+    // JV: The local parameterization projects the covariance to the state size.
     ASSERT_EQ(Cov.cols() , 7);
     ASSERT_EQ(Cov.rows() , 7);
+    ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12);
 
 }
 
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 8a966f77b39390e80ad2ad5e68655bbb764ebf85..822fc5b816eda32d729067f0476f8dd4b98b6ac5 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -8,12 +8,12 @@
 #include "base/capture/capture_IMU.h"
 #include "base/processor/processor_IMU.h"
 #include "base/sensor/sensor_IMU.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/rotations.h"
+#include "base/math/rotations.h"
 #include "base/ceres_wrapper/ceres_manager.h"
 
 #include <cmath>
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
index 1e3b0940cdf2434a059b071b95ca47847ebadd48..627549043e314e3861763eb4ba1253a6631e38ba 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_IMU_jacobians.cpp
@@ -9,10 +9,10 @@
 #include "base/capture/capture_IMU.h"
 #include "base/sensor/sensor_IMU.h"
 #include "test/processor_IMU_UnitTester.h"
-#include "base/wolf.h"
-#include "base/problem.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
+#include "base/common/wolf.h"
+#include "base/problem/problem.h"
+#include "base/state_block/state_block.h"
+#include "base/state_block/state_quaternion.h"
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 7479ca984eda6dff7504595c66a959ae1d3aebfa..904dfd7646f1194892e0845b003cd20d17916c4b 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -14,7 +14,7 @@
 #include "base/processor/processor_tracker_feature_dummy.h"
 #include "base/capture/capture_void.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 
 // STL
 #include <iterator>
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 425f6a96b9a9af4dcdcf5a491a8fb2ac75f98cf3..cfbdd69e0f978d8822bf0e707ff48a2a3fcefbbd 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -7,7 +7,7 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
 #include "base/sensor/sensor_odom_2D.h"
 #include "base/processor/processor_frame_nearest_neighbor_filter.h"
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 205c79f9d60a78a4b006e204b6537adea720274c..0a02d13a13dbd9d25871cbc718aaaf3bb3902568 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -7,8 +7,8 @@
 
 #include "utils_gtest.h"
 
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
 
 #include "base/sensor/sensor_odom_2D.h"
 #include "base/processor/processor_odom_2D.h"
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 6819cba18e0ec605a6272e8ac6e05e44cf58a6d3..d7b13f0c404a1c86f8b5c92ac1a55f5682dd89c6 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -1,7 +1,7 @@
 #include "utils_gtest.h"
 
-#include "base/wolf.h"
-#include "base/logging.h"
+#include "base/common/wolf.h"
+#include "base/utils/logging.h"
 
 #include "vision_utils.h"
 
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 5d656eda2fe23cde71e16b1ca15c389e11d65940..fb4583063808187c7e8ed1feb8fcaf9ad7f43eb2 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -9,8 +9,8 @@
 #include <Eigen/Geometry>
 
 //Wolf
-#include "base/wolf.h"
-#include "base/rotations.h"
+#include "base/common/wolf.h"
+#include "base/math/rotations.h"
 
 //std
 #include <iostream>
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index cebdce1f8da148e95c39422c422d9a44422c3dbe..9c9055ff5d3f7911d4d87b83e44f77e4bcc5b163 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "base/node_base.h"
+#include "base/common/node_base.h"
 
 class CChildBase;
 
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03..be68ea5ec11cbad46c3154c874ce9bec29d97c33 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -6,16 +6,16 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/problem.h"
+#include "base/problem/problem.h"
 #include "base/sensor/sensor_base.h"
-#include "base/state_block.h"
+#include "base/state_block/state_block.h"
 #include "base/capture/capture_void.h"
 #include "base/factor/factor_pose_2D.h"
 #include "base/solver/solver_manager.h"
-#include "base/local_parametrization_base.h"
-#include "base/local_parametrization_angle.h"
+#include "base/state_block/local_parametrization_base.h"
+#include "base/state_block/local_parametrization_angle.h"
 
 #include <iostream>
 
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index aad321781d7682c8c60dd83c9e0970e17118b164..a8389ce8b36f453a759ba16000ec106fe68160a3 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "base/time_stamp.h"
+#include "base/common/time_stamp.h"
 
 #include <thread>
 
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index e3d533a5fb31f4f524cde8630446ba0e5dda259b..99d47c9c3ffd95da4c6f98f6cefd0d4da07c4ed0 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -7,7 +7,7 @@
 
 #include "utils_gtest.h"
 
-#include "base/track_matrix.h"
+#include "base/processor/track_matrix.h"
 
 using namespace wolf;
 
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index d77defcbc697d87cf827a465d1ef99e9221293a6..e74519b58be66ea7efb68f805e3e3e15e022e0d7 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -6,11 +6,11 @@
  */
 
 #include "utils_gtest.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
-#include "base/problem.h"
-#include "base/trajectory_base.h"
-#include "base/frame_base.h"
+#include "base/problem/problem.h"
+#include "base/trajectory/trajectory_base.h"
+#include "base/frame/frame_base.h"
 
 #include <iostream>
 
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
index c114086a16cae9cb28859270b3784461d7bb7773..2538981e0fcba72cd2c0b81901445363fd207a37 100644
--- a/test/processor_IMU_UnitTester.h
+++ b/test/processor_IMU_UnitTester.h
@@ -196,8 +196,8 @@ namespace wolf {
 /////////////////////////////////////////////////////////
 
 // Wolf
-#include "base/state_block.h"
-#include "base/rotations.h"
+#include "base/state_block/state_block.h"
+#include "base/math/rotations.h"
 
 namespace wolf{
 
diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
index 8532cc9f2d32246ed536a340574251a6b34ce379..ae4a43a3bdd7733931da9b42fb87e279f529a7a2 100644
--- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
+++ b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
@@ -5,7 +5,7 @@
  *      Author: Jeremie Deray
  */
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "../../utils_gtest.h"
 
 #include "../../../serialization/cereal/serialization_eigen_geometry.h"
diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
index 5ca60e7f111f5b9e56392bcf59af3123c29637b6..c7f7249a5747d2e848bf07ea892b8486ee1f2eaa 100644
--- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
+++ b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
@@ -5,7 +5,7 @@
  *      Author: Jeremie Deray
  */
 
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 #include "../../utils_gtest.h"
 
 #include "../../../serialization/cereal/serialization_eigen_sparse.h"
diff --git a/wolf_scripts/include_refactor.sh b/wolf_scripts/include_refactor.sh
index 6d2c2fd8d6fae80bb3c327ead22dfa4c097de20b..2801608bc3747dc71915205f13833c4a6ab36d23 100755
--- a/wolf_scripts/include_refactor.sh
+++ b/wolf_scripts/include_refactor.sh
@@ -1,17 +1,36 @@
 #!/bin/bash
-for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do
-    for f in $(cat ~/workspace/wip/wolf/files.txt); do
-        path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-)
-        matches=$(echo $path | wc -w)
-        if [ $matches -gt 1 ]; then
-            # echo $f " -> " $path
-            path=$(echo $path | cut -d ' ' -f 1)
-        fi
-        # echo $f " now in -> " $path " modifying file "$ff
-        # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff
-        sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff
+for folder in problem hardware trajectory map frame state_block common math utils; do
+    for ff in $(find include/base/$folder src/$folder -type f); do
+        name=$(echo $ff | rev | cut -d '/' -f 1 | rev)
+        old="base/$name"
+        new="base/$folder/$name"
+        # echo "%%%%%%%%% "$ff " ¬¬ $name"
+        # echo "$old ºº $new"
+        # for target in $(find include/base src test -type f); do
+        for target in $(find hello_wolf -type f); do
+            # out=$(sed -E -n "s:$old:$new:gp" $target)
+            out=$(sed -i -E "s:$old:$new:g" $target)
+            if [[ $out ]]; then
+                echo ">>> changing : $old -> $new @ $target"
+                echo $out
+            fi
+        done
     done
 done
+
+# for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do
+#     for f in $(cat ~/workspace/wip/wolf/files.txt); do
+#         path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-)
+#         matches=$(echo $path | wc -w)
+#         if [ $matches -gt 1 ]; then
+#             # echo $f " -> " $path
+#             path=$(echo $path | cut -d ' ' -f 1)
+#         fi
+#         # echo $f " now in -> " $path " modifying file "$ff
+#         # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff
+#         sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff
+#     done
+# done
 # for f in $(cat ~/workspace/wip/wolf/files.txt); do
 #     path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 7-)
 #     matches=$(echo $path | wc -w)