diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8d4e1e7e0661b09326ccc803a22d732916081879..91de281b3779c42c55c3a68359c02e3bcadf24c7 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 ?
@@ -255,40 +255,55 @@ ENDIF(GLOG_FOUND)
 #HEADERS
 
 SET(HDRS_BASE
-  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/IMU_tools.h
-  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
+  )
+
+SET(HDRS_COMMON
+  include/base/factory.h
   include/base/node_base.h
+  include/base/time_stamp.h
+  include/base/wolf.h
+  )
+SET(HDRS_MATH
   include/base/pinhole_tools.h
-  include/base/problem.h
+  include/base/make_unique.h
+  include/base/SE3.h
+  )
+SET(HDRS_UTILS
+  include/base/utils/eigen_assert.h
+  include/base/utils/eigen_predicates.h
   include/base/rotations.h
+  include/base/logging.h
   include/base/singleton.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/local_parametrization_homogeneous.h
+  include/base/local_parametrization_polyline_extreme.h
+  include/base/local_parametrization_quaternion.h
   include/base/state_angle.h
   include/base/state_block.h
   include/base/state_homogeneous_3D.h
   include/base/state_quaternion.h
-  include/base/SE3.h
-  include/base/time_stamp.h
-  include/base/track_matrix.h
-  include/base/trajectory_base.h
-  include/base/wolf.h
-  include/base/IMU_tools.h
   include/base/local_parametrization_polyline_extreme.h
   )
+
 SET(HDRS_CAPTURE
   include/base/capture/capture_base.h
   include/base/capture/capture_buffer.h
@@ -383,29 +398,33 @@ SET(HDRS_LANDMARK
   include/base/landmark/landmark_polyline_2D.h
   )
 SET(HDRS_PROCESSOR
+  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
   include/base/processor/processor_diff_drive.h
+  include/base/processor/processor_factory.h
+  include/base/processor/processor_factory.h
   include/base/processor/processor_frame_nearest_neighbor_filter.h
-  include/base/processor/processor_IMU.h
+  include/base/processor/processor_frame_nearest_neighbor_filter.h
+  include/base/processor/processor_logging.h
+  include/base/processor/processor_loopclosure_base.h
+  include/base/processor/processor_motion.h
+  include/base/processor/processor_odom_2D.h
   include/base/processor/processor_odom_2D.h
   include/base/processor/processor_odom_3D.h
+  include/base/processor/processor_odom_3D.h
+  include/base/processor/processor_tracker.h
+  include/base/processor/processor_tracker_feature.h
+  include/base/processor/processor_tracker_feature_dummy.h
   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_frame_nearest_neighbor_filter.h
-  include/base/processor/processor_IMU.h
-  include/base/processor/processor_odom_2D.h
-  include/base/processor/processor_odom_3D.h
-  include/base/processor/processor_tracker_feature_dummy.h
   include/base/processor/processor_tracker_landmark_dummy.h
-  include/base/processor/processor_factory.h
-  include/base/processor/processor_logging.h
-  include/base/processor/processor_base.h
-  include/base/processor/processor_factory.h
-  include/base/processor/processor_loopclosure_base.h
-  include/base/processor/processor_motion.h
-  include/base/processor/processor_tracker_feature.h
-  include/base/processor/processor_tracker.h
+  include/base/processor/track_matrix.h
   )
 SET(HDRS_SENSOR
   include/base/sensor/sensor_base.h
@@ -427,101 +446,55 @@ 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
   include/base/association/association_nnls.h
   )
 
-SET(HDRS_CORE 
-  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/processor/processor_logging.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_loopclosure_base.h
-  include/base/processor/processor_motion.h
-  include/base/processor/processor_tracker_feature.h
-  include/base/processor/processor_tracker.h
-  include/base/rotations.h
-  include/base/sensor/sensor_base.h
-  include/base/processor/processor_factory.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/SE3.h
-  include/base/time_stamp.h
-  include/base/track_matrix.h
-  include/base/trajectory_base.h
-  include/base/wolf.h
-  )
 
 #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_PROBLEM
+  src/problem/problem.cpp
   )
-
-SET(SRCS_BASE
-  src/capture/capture_motion.cpp
-  src/processor/processor_capture_holder.cpp
-  # examples/test_processor_tracker_landmark.cpp
+SET(SRCS_HARDWARE
+  src/hardware/hardware_base.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
@@ -529,33 +502,48 @@ 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_corner_2D.cpp
-  src/feature/feature_diff_drive.cpp
   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/processor_frame_nearest_neighbor_filter.cpp
-  src/processor/processor_diff_drive.cpp
+  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_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
@@ -704,17 +692,25 @@ ENDIF(YAMLCPP_FOUND)
 ADD_LIBRARY(${PROJECT_NAME} 
   SHARED 
   ${SRCS_BASE} 
-  ${SRCS_CORE}
-  ${SRCS}
   ${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}
   )
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 
@@ -770,6 +766,24 @@ INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
 
 install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
 #install headers
+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_BASE}
     DESTINATION include/iri-algorithms/wolf/base)
 INSTALL(FILES ${HDRS_DTASSC}
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 7eabd88301bad8fdb4e5297e259c410353241f32..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"
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index 04d704664e8023826fb0b3faccde05a585a7b47a..591800ae63b6a2a19ed05e6d35358747ba6fc8b1 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/IMU_tools.h b/include/base/IMU_tools.h
index eecad244c7a57ee2868bcf464e951a0a5108160d..815ed2aa23098b6c0c9d372666f2c2dedae3850e 100644
--- a/include/base/IMU_tools.h
+++ b/include/base/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/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 99%
rename from include/base/wolf.h
rename to include/base/common/wolf.h
index 7913365951aea336b637fa962f5e73e9b7ad20d7..6b73e59a24673b4267dccd85f7ce81ec8ce68b9e 100644
--- a/include/base/wolf.h
+++ b/include/base/common/wolf.h
@@ -10,7 +10,7 @@
 
 // Enable project-specific definitions and macros
 #include "internal/config.h"
-#include "base/logging.h"
+#include "base/utils/logging.h"
 
 //includes from Eigen lib
 #include <Eigen/Dense>
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 0177bd8e9253dbd9840ec8fe3a765720a0754fc6..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
 
@@ -155,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 e3b61d65bf86a7690a9350d3d089bec156637092..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
 
@@ -96,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/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 99%
rename from include/base/problem.h
rename to include/base/problem/problem.h
index a9f8e5e4a28c857a4061ec2296cb81cf31858dfa..8c88b0eaa2283044b2457c79649290625daaade4 100644
--- a/include/base/problem.h
+++ b/include/base/problem/problem.h
@@ -14,9 +14,9 @@ struct ProcessorParamsBase;
 }
 
 //wolf includes
-#include "base/wolf.h"
-#include "base/frame_base.h"
-#include "base/state_block.h"
+#include "base/common/wolf.h"
+#include "base/frame/frame_base.h"
+#include "base/state_block/state_block.h"
 
 // std includes
 
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/processor_base.h b/include/base/processor/processor_base.h
index 84fee5343c63ec3c01b28de1dfbc46c37125f337..5d68c43c7b921532dc1d1d0d32ddabe89d75de93 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,10 +7,10 @@ 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/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 d0c8411f285b11b1f727eccd813f80ae092b9fbb..4ea5181b81321b4649f1e2bdf835db2e0e820959 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -9,7 +9,7 @@
 #define _WOLF_PROCESSOR_DIFF_DRIVE_H_
 
 #include "base/processor/processor_motion.h"
-#include "base/diff_drive_tools.h"
+#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 be1205e27988461473786627ca5e0d3acc62748f..ceb49425395712ff35d2c0e1d61c0882eecfc84c 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -3,7 +3,7 @@
 
 // Wolf related headers
 #include "base/processor/processor_loopclosure_base.h"
-#include "base/state_block.h"
+#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 b66feb38face40a05ce5c96696920ece1ca86c1b..46af97f524059b9c5590a40d569d9f8f5c1c2acc 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -11,7 +11,7 @@
 // Wolf
 #include "base/capture/capture_motion.h"
 #include "base/processor/processor_base.h"
-#include "base/time_stamp.h"
+#include "base/common/time_stamp.h"
 
 // std
 #include <iomanip>
@@ -476,7 +476,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 d18c9591e5abd9932a7781144f172b9a72826487..ae73d0e93662231dc0a77f4e5c26f26fddda7160 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -11,7 +11,7 @@
 #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/math/rotations.h"
 
 namespace wolf {
     
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 2cbc26a068f13c8e316bf876dd8d483b7eb0a125..5602eb3839cabffcb83ebde39f0ef7d8a74c8c49 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_feature.h b/include/base/processor/processor_tracker_feature.h
index c743eed17a62e6f506ac0d7adb01354ee4d4d011..17da3af43bd37be3b7f8543aa539554e1d50eb83 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
 {
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 63ef69245d53cb14520d8f5b16759b8b34b230d2..566b2aea9ad081f14598c19a41746ce5b2917efc 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"
 
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 419c31f5d31b81211c2b9cf15048c559103356ae..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"
 
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 25e2c629157033508b32bba6e80c910510ca8e16..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"
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index 83be3c837aa8c2f26f57ceb8322a3897303096ad..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"
 
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index afdc8eff9200218c2856ee397aac879960a146d2..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>
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/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index a56b8c26776e054dc469d563b113df7c4b68c4d3..6881537b62e614bc11b51592abb9f7ce89b67c6a 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 64ed42ba22597606c616af57c9ba23984ad3b2a7..4d9910ff28b126af81972b62aac55a07ad947ddd 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
 
@@ -190,8 +190,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 0f5cf42592ddf2cf1929ff6e31a9aa294a1963e4..4a38829740849938b7ca0388e54d158cf6ee46bb 100644
--- a/include/base/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -10,7 +10,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/diff_drive_tools.h"
+#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 829393980aa4093dfd86438ece963a58eba8a85c..08d05aaa19abbfedee7bde87dc9450630ea16fe5 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 97%
rename from src/frame_base.cpp
rename to src/frame/frame_base.cpp
index 32c65515203711405d50b7b95946a76d3dbe10bd..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 {
 
@@ -374,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 705cb97b45ef6796d1441630c010e366f657d281..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 {
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 99%
rename from src/problem.cpp
rename to src/problem/problem.cpp
index 4fe1df930f72885805988ad8038bbe760e865982..a97f935f2c741294bc95cd9c3c0a9ec4a4884fb2 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
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_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_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 8516719fe6ea6b1bf02ab72cf4fb5f92d3d433a7..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>
 
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index c3cac8003660aa14149fe1fa4ba2e01927772312..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
 {
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index 6d6ca0cf23ef88cfddfefac7d4de28b2c51004d4..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>
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 c698db52f69d668ca7bab3c522087cb168047d10..dc3772d7b250183d99a153acf6f22e04a089e1f4 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_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 df5d418e017d2862941212fd3f94ff173fdaf13a..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
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 e7a1d6a32d705fef8c5085ef475e3a358a4624e7..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"
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)