From e1adf38ae6e60897fde5cf958891c5fcee5e0d27 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Fri, 12 Apr 2019 14:14:48 +0200
Subject: [PATCH] Code reorganization, added folders, moved some files.

---
 CMakeLists.txt                                | 262 +++++++++---------
 hello_wolf/hello_wolf.cpp                     |   2 +-
 hello_wolf/sensor_range_bearing.cpp           |   2 +-
 include/base/IMU_tools.h                      |   4 +-
 include/base/capture/capture_base.h           |  10 +-
 include/base/capture/capture_buffer.h         |   4 +-
 include/base/capture/capture_motion.h         |   2 +-
 include/base/capture/capture_odom_2D.h        |   2 +-
 include/base/capture/capture_odom_3D.h        |   2 +-
 .../ceres_wrapper/cost_function_wrapper.h     |   2 +-
 .../local_parametrization_wrapper.h           |   4 +-
 include/base/ceres_wrapper/solver_manager.h   |   4 +-
 include/base/{ => common}/factory.h           |   4 +-
 include/base/{ => common}/node_base.h         |   2 +-
 include/base/{ => common}/time_stamp.h        |   2 +-
 include/base/{ => common}/wolf.h              |   2 +-
 include/base/factor/factor_AHP.h              |   2 +-
 include/base/factor/factor_GPS_2D.h           |   4 +-
 include/base/factor/factor_IMU.h              |   2 +-
 include/base/factor/factor_autodiff.h         |   2 +-
 .../base/factor/factor_autodiff_trifocal.h    |   4 +-
 include/base/factor/factor_base.h             |   8 +-
 include/base/factor/factor_block_absolute.h   |   2 +-
 include/base/factor/factor_container.h        |   2 +-
 include/base/factor/factor_diff_drive.h       |   2 +-
 include/base/factor/factor_fix_bias.h         |   4 +-
 include/base/factor/factor_odom_2D.h          |   2 +-
 include/base/factor/factor_odom_3D.h          |   2 +-
 include/base/factor/factor_pose_2D.h          |   4 +-
 include/base/factor/factor_pose_3D.h          |   4 +-
 .../base/factor/factor_quaternion_absolute.h  |   6 +-
 include/base/feature/feature_IMU.h            |   2 +-
 include/base/feature/feature_base.h           |   4 +-
 include/base/feature/feature_match.h          |   2 +-
 include/base/{ => frame}/frame_base.h         |  10 +-
 include/base/{ => hardware}/hardware_base.h   |   4 +-
 include/base/landmark/landmark_base.h         |  10 +-
 include/base/landmark/landmark_container.h    |   2 +-
 include/base/landmark/landmark_line_2D.h      |   2 +-
 include/base/landmark/landmark_match.h        |   2 +-
 include/base/{ => map}/map_base.h             |   4 +-
 include/base/{ => math}/SE3.h                 |   4 +-
 include/base/{ => math}/pinhole_tools.h       |   2 +-
 include/base/{ => math}/rotations.h           |   2 +-
 include/base/{ => problem}/problem.h          |   6 +-
 .../base/{ => processor}/diff_drive_tools.h   |   4 +-
 .../base/{ => processor}/diff_drive_tools.hpp |   0
 include/base/{ => processor}/motion_buffer.h  |   4 +-
 include/base/processor/processor_base.h       |   8 +-
 include/base/processor/processor_diff_drive.h |   2 +-
 include/base/processor/processor_factory.h    |   4 +-
 .../processor_frame_nearest_neighbor_filter.h |   2 +-
 include/base/processor/processor_logging.h    |   2 +-
 include/base/processor/processor_motion.h     |   4 +-
 include/base/processor/processor_odom_2D.h    |   2 +-
 include/base/processor/processor_odom_3D.h    |   2 +-
 .../processor/processor_tracker_feature.h     |   4 +-
 .../processor_tracker_feature_corner.h        |   2 +-
 .../processor_tracker_feature_dummy.h         |   2 +-
 .../processor_tracker_feature_image.h         |   4 +-
 .../processor_tracker_landmark_corner.h       |   2 +-
 .../processor_tracker_landmark_image.h        |   2 +-
 include/base/{ => processor}/track_matrix.h   |   0
 include/base/sensor/sensor_GPS.h              |   2 +-
 include/base/sensor/sensor_base.h             |  10 +-
 include/base/sensor/sensor_diff_drive.h       |   2 +-
 include/base/sensor/sensor_factory.h          |   2 +-
 include/base/solver/solver_manager.h          |   4 +-
 .../solver_suitesparse/cost_function_base.h   |   2 +-
 .../solver_suitesparse/cost_function_sparse.h |   2 +-
 .../cost_function_sparse_base.h               |   2 +-
 include/base/solver_suitesparse/qr_solver.h   |   2 +-
 .../base/solver_suitesparse/solver_manager.h  |   4 +-
 .../local_parametrization_angle.h             |   4 +-
 .../local_parametrization_base.h              |   2 +-
 .../local_parametrization_homogeneous.h       |   2 +-
 .../local_parametrization_polyline_extreme.h  |   2 +-
 .../local_parametrization_quaternion.h        |   2 +-
 include/base/{ => state_block}/state_angle.h  |   4 +-
 include/base/{ => state_block}/state_block.h  |   8 +-
 .../{ => state_block}/state_homogeneous_3D.h  |   4 +-
 .../base/{ => state_block}/state_quaternion.h |   4 +-
 .../base/{ => trajectory}/trajectory_base.h   |   4 +-
 include/base/{ => utils}/eigen_assert.h       |   0
 include/base/{ => utils}/eigen_predicates.h   |   2 +-
 include/base/{ => utils}/logging.h            |   2 +-
 include/base/{ => utils}/make_unique.h        |   0
 include/base/{ => utils}/singleton.h          |   0
 ...serialization_local_parametrization_base.h |   2 +-
 src/capture/capture_IMU.cpp                   |   2 +-
 src/capture/capture_wheel_joint_position.cpp  |   2 +-
 src/ceres_wrapper/ceres_manager.cpp           |   6 +-
 src/ceres_wrapper/solver_manager.cpp          |   4 +-
 src/{ => common}/node_base.cpp                |   2 +-
 src/{ => common}/time_stamp.cpp               |   2 +-
 src/examples/solver/test_ccolamd.cpp          |   2 +-
 src/examples/solver/test_iQR_wolf2.cpp        |   2 +-
 src/examples/test_2_lasers_offline.cpp        |   2 +-
 src/examples/test_ceres_2_lasers.cpp          |   2 +-
 .../test_ceres_2_lasers_polylines.cpp         |   2 +-
 src/examples/test_diff_drive.cpp              |   4 +-
 src/examples/test_eigen_quaternion.cpp        |   2 +-
 src/examples/test_factor_AHP.cpp              |   6 +-
 src/examples/test_factor_imu.cpp              |   8 +-
 src/examples/test_faramotics_simulation.cpp   |   4 +-
 src/examples/test_imuDock.cpp                 |   4 +-
 src/examples/test_imuDock_autoKFs.cpp         |   4 +-
 src/examples/test_imuPlateform_Offline.cpp    |   8 +-
 src/examples/test_imu_constrained0.cpp        |   8 +-
 src/examples/test_map_yaml.cpp                |   8 +-
 src/examples/test_mpu.cpp                     |   8 +-
 src/examples/test_processor_imu.cpp           |   8 +-
 src/examples/test_processor_imu_jacobians.cpp |   8 +-
 src/examples/test_processor_odom_3D.cpp       |   4 +-
 .../test_processor_tracker_feature.cpp        |   6 +-
 .../test_processor_tracker_landmark.cpp       |   6 +-
 .../test_processor_tracker_landmark_image.cpp |   6 +-
 src/examples/test_projection_points.cpp       |   2 +-
 src/examples/test_simple_AHP.cpp              |   8 +-
 src/examples/test_sort_keyframes.cpp          |   8 +-
 src/examples/test_state_quaternion.cpp        |   6 +-
 src/examples/test_wolf_factories.cpp          |   6 +-
 src/examples/test_wolf_logging.cpp            |   4 +-
 src/examples/test_wolf_root.cpp               |   2 +-
 src/examples/test_yaml.cpp                    |   4 +-
 src/factor/factor_analytic.cpp                |   2 +-
 src/factor/factor_base.cpp                    |   2 +-
 src/{ => frame}/frame_base.cpp                |  12 +-
 src/{ => hardware}/hardware_base.cpp          |   2 +-
 src/landmark/landmark_AHP.cpp                 |   4 +-
 src/landmark/landmark_base.cpp                |   4 +-
 src/landmark/landmark_container.cpp           |   2 +-
 src/landmark/landmark_polyline_2D.cpp         |   6 +-
 src/{ => map}/map_base.cpp                    |   4 +-
 src/{ => problem}/problem.cpp                 |  10 +-
 src/{ => processor}/motion_buffer.cpp         |   2 +-
 src/processor/processor_base.cpp              |   2 +-
 src/processor/processor_diff_drive.cpp        |   2 +-
 src/processor/processor_tracker_landmark.cpp  |   2 +-
 .../processor_tracker_landmark_corner.cpp     |   2 +-
 .../processor_tracker_landmark_image.cpp      |  14 +-
 src/{ => processor}/track_matrix.cpp          |   2 +-
 src/sensor/sensor_GPS.cpp                     |   4 +-
 src/sensor/sensor_GPS_fix.cpp                 |   4 +-
 src/sensor/sensor_IMU.cpp                     |   4 +-
 src/sensor/sensor_base.cpp                    |   4 +-
 src/sensor/sensor_camera.cpp                  |   6 +-
 src/sensor/sensor_diff_drive.cpp              |   4 +-
 src/sensor/sensor_laser_2D.cpp                |   2 +-
 src/sensor/sensor_odom_2D.cpp                 |   4 +-
 src/sensor/sensor_odom_3D.cpp                 |   4 +-
 src/solver/solver_manager.cpp                 |   4 +-
 .../local_parametrization_base.cpp            |   2 +-
 .../local_parametrization_homogeneous.cpp     |   4 +-
 ...local_parametrization_polyline_extreme.cpp |   6 +-
 .../local_parametrization_quaternion.cpp      |   4 +-
 src/{ => state_block}/state_block.cpp         |   2 +-
 src/{ => trajectory}/trajectory_base.cpp      |   4 +-
 src/yaml/processor_IMU_yaml.cpp               |   2 +-
 src/yaml/processor_image_yaml.cpp             |   2 +-
 src/yaml/processor_odom_3D_yaml.cpp           |   2 +-
 ...rocessor_tracker_feature_trifocal_yaml.cpp |   2 +-
 src/yaml/sensor_IMU_yaml.cpp                  |   2 +-
 src/yaml/sensor_camera_yaml.cpp               |   2 +-
 src/yaml/sensor_laser_2D_yaml.cpp             |   2 +-
 src/yaml/sensor_odom_3D_yaml.cpp              |   2 +-
 test/gtest_IMU.cpp                            |   4 +-
 test/gtest_SE3.cpp                            |   2 +-
 test/gtest_capture_base.cpp                   |   2 +-
 test/gtest_ceres_manager.cpp                  |  10 +-
 test/gtest_eigen_predicates.cpp               |   2 +-
 test/gtest_factor_IMU.cpp                     |   2 +-
 test/gtest_factor_autodiff_distance_3D.cpp    |   6 +-
 test/gtest_factor_autodiff_trifocal.cpp       |   2 +-
 test/gtest_feature_IMU.cpp                    |  10 +-
 test/gtest_frame_base.cpp                     |   6 +-
 test/gtest_local_param.cpp                    |  10 +-
 test/gtest_make_posdef.cpp                    |   2 +-
 test/gtest_motion_buffer.cpp                  |   6 +-
 test/gtest_odom_2D.cpp                        |   4 +-
 test/gtest_odom_3D.cpp                        |   4 +-
 test/gtest_pack_KF_buffer.cpp                 |   2 +-
 test/gtest_param_prior.cpp                    |   4 +-
 test/gtest_pinhole.cpp                        |   2 +-
 test/gtest_problem.cpp                        |   4 +-
 test/gtest_processor_IMU.cpp                  |   6 +-
 test/gtest_processor_IMU_jacobians.cpp        |   8 +-
 test/gtest_processor_base.cpp                 |   2 +-
 ...essor_frame_nearest_neighbor_filter_2D.cpp |   2 +-
 test/gtest_processor_motion.cpp               |   4 +-
 ...est_processor_tracker_feature_trifocal.cpp |   4 +-
 test/gtest_rotation.cpp                       |   4 +-
 test/gtest_shared_from_this.cpp               |   2 +-
 test/gtest_solver_manager.cpp                 |  10 +-
 test/gtest_time_stamp.cpp                     |   2 +-
 test/gtest_track_matrix.cpp                   |   2 +-
 test/gtest_trajectory.cpp                     |   8 +-
 test/processor_IMU_UnitTester.h               |   4 +-
 .../gtest_serialization_eigen_geometry.cpp    |   2 +-
 .../gtest_serialization_eigen_sparse.cpp      |   2 +-
 wolf_scripts/include_refactor.sh              |  41 ++-
 201 files changed, 541 insertions(+), 508 deletions(-)
 rename include/base/{ => common}/factory.h (99%)
 rename include/base/{ => common}/node_base.h (99%)
 rename include/base/{ => common}/time_stamp.h (99%)
 rename include/base/{ => common}/wolf.h (99%)
 rename include/base/{ => frame}/frame_base.h (97%)
 rename include/base/{ => hardware}/hardware_base.h (91%)
 rename include/base/{ => map}/map_base.h (93%)
 rename include/base/{ => math}/SE3.h (99%)
 rename include/base/{ => math}/pinhole_tools.h (99%)
 rename include/base/{ => math}/rotations.h (99%)
 rename include/base/{ => problem}/problem.h (99%)
 rename include/base/{ => processor}/diff_drive_tools.h (99%)
 rename include/base/{ => processor}/diff_drive_tools.hpp (100%)
 rename include/base/{ => processor}/motion_buffer.h (98%)
 rename include/base/{ => processor}/track_matrix.h (100%)
 rename include/base/{ => state_block}/local_parametrization_angle.h (95%)
 rename include/base/{ => state_block}/local_parametrization_base.h (97%)
 rename include/base/{ => state_block}/local_parametrization_homogeneous.h (97%)
 rename include/base/{ => state_block}/local_parametrization_polyline_extreme.h (95%)
 rename include/base/{ => state_block}/local_parametrization_quaternion.h (97%)
 rename include/base/{ => state_block}/state_angle.h (84%)
 rename include/base/{ => state_block}/state_block.h (98%)
 rename include/base/{ => state_block}/state_homogeneous_3D.h (91%)
 rename include/base/{ => state_block}/state_quaternion.h (92%)
 rename include/base/{ => trajectory}/trajectory_base.h (96%)
 rename include/base/{ => utils}/eigen_assert.h (100%)
 rename include/base/{ => utils}/eigen_predicates.h (99%)
 rename include/base/{ => utils}/logging.h (99%)
 rename include/base/{ => utils}/make_unique.h (100%)
 rename include/base/{ => utils}/singleton.h (100%)
 rename src/{ => common}/node_base.cpp (75%)
 rename src/{ => common}/time_stamp.cpp (97%)
 rename src/{ => frame}/frame_base.cpp (97%)
 rename src/{ => hardware}/hardware_base.cpp (93%)
 rename src/{ => map}/map_base.cpp (97%)
 rename src/{ => problem}/problem.cpp (99%)
 rename src/{ => processor}/motion_buffer.cpp (99%)
 rename src/{ => processor}/track_matrix.cpp (99%)
 rename src/{ => state_block}/local_parametrization_base.cpp (88%)
 rename src/{ => state_block}/local_parametrization_homogeneous.cpp (93%)
 rename src/{ => state_block}/local_parametrization_polyline_extreme.cpp (93%)
 rename src/{ => state_block}/local_parametrization_quaternion.cpp (97%)
 rename src/{ => state_block}/state_block.cpp (94%)
 rename src/{ => trajectory}/trajectory_base.cpp (97%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8d4e1e7e0..91de281b3 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 7eabd8830..3708f51d3 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 04d704664..591800ae6 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 eecad244c..815ed2aa2 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 0082d9d31..b3adeb9be 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 434716dcf..393d7fc24 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 ed71171e9..0f84eb72d 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 eafb6ead7..ff532803c 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 d1f29508b..863add268 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 4a3f42dcb..2847d9b9a 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 fc046a7ec..31a2913ec 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 7252c409d..1bdf40a64 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 b161126e4..03b636e81 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 ebbc64082..7ecd72166 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 02a5599af..14ed594f6 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 791336595..6b73e59a2 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 a6448b064..efb0837c8 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 a3ce0e609..bac2e381c 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 fbb29d164..942cbf51c 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 be35c772a..372891cdd 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 d7c2091fb..5b88708c4 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 4562d5f75..8d79b0065 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 4e0da9d76..3990a4412 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 93d455a8e..7f34ad69f 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 b495859c7..84f535ee7 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 6c1f321dc..82a137969 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 e45570d79..8a9d06747 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 ce3456ef0..c3a3cfb83 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 306b5e15e..f1cc286ea 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 b9c4da395..25273bad2 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 1864a6c8e..abb419afa 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 dfce11ff7..c82083332 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 3732bc552..3e1ea5588 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 b96ce0023..92e456019 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 0177bd8e9..ffa84d74d 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 6dd60648b..fb688410b 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 e3b61d65b..dad1e3a7c 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 65a2c5b71..122b77071 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 3a0f84c34..7caf3b290 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 0c5d3a491..48de1e5c9 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 a8b447905..c3d6eb8c9 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 cea354607..5a84f0c7e 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 46cb1fb9d..9fed8ba7c 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 d4d3e711f..fc0e8b2bb 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 a9f8e5e4a..8c88b0eaa 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 142b8c502..92883eafa 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 f8d0ad5be..40fbd2925 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 84fee5343..5d68c43c7 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 d0c8411f2..4ea5181b8 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 8077aae49..390a66bec 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 be1205e27..ceb494253 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 4add7a8e9..d41d7ce30 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 b66feb38f..46af97f52 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 d18c9591e..ae73d0e93 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 2cbc26a06..5602eb383 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 c743eed17..17da3af43 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 63ef69245..566b2aea9 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 419c31f5d..c0b40edbe 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 25e2c6291..7378c8a3d 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 83be3c837..2b381719a 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 afdc8eff9..f98f87707 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 a56b8c267..6881537b6 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 64ed42ba2..4d9910ff2 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 0f5cf4259..4a3882974 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 32884c18f..d74c9b1ab 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 f64b35719..28e43acfa 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 8aae707ae..25d891b95 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 bdcde3834..3ed0eb5e3 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 9aeff876c..d90a0577e 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 154b78639..0d7f49ccd 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 09c0abe7f..3d1ef2a27 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 912237c07..3644c83ec 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 3b7dcc38d..cbc0cbb13 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 f06dd2fb1..cd2076d63 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 3a75a8a0f..0e0cc29cd 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 f92676558..c28a05d49 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 c61286e13..ae5133b32 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 447e8a0e9..86c94d6ce 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 adfda9018..369e14f43 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 d990ce1f8..f7ad39f2d 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 6ceddf9e6..37f9762c7 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 55f2eeeb7..bc0ab9322 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 3593fc718..088a82152 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 0c38a5a61..1385e1af0 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 6dbc07d59..8bfc7af04 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 7b11d9c8d..1531820a0 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 829393980..08d05aaa1 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 06ae2d113..72e6e556b 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 24cb56c20..882e70a69 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 55d25fc6e..3e84bf846 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 2fae213bc..d9eec5b00 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 93b39fe65..dc4094304 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 10824d51e..9174dc94f 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 d4afdc7d0..d12f491de 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 9e9ca5390..136ba285e 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 306c7dfdc..23980071f 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 1c01f5e13..d24a715cb 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 b3938285e..75031dff8 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 5dde0e16a..3d0483e96 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 621f65186..e095e561d 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 dff0efd51..dab6894d7 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 4415b685c..43a54d654 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 e7ed6a915..4b923de89 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 f134ccc12..e542e18e0 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 7dedb1200..2dd5ab64d 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 0e397cac3..1fbed214f 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 7dc68ebd7..33ec4cabb 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 6cf92cef2..22c797d6d 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 ffeb5ce92..79798150d 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 ab22be418..77e2b35e9 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 81900c7ef..170222976 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 d8b22c18d..be7df536e 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 c24e8904e..b9f01912d 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 142b99808..6bd1805dc 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 e046765f0..56982c9dc 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 5885553ed..0fabac44e 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 b35d0dc1b..b608bb958 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 b516acd09..3b7bdfab7 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 a2e3ccef5..ff78c97b0 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 cfd03d18d..5181d73ac 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 c86bec4aa..7ad85937a 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 e3dc7b1ab..188e873b6 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 32c655152..cb7f089c1 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 945b412c8..69b23c34c 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 1daa37470..4fde7a42c 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 705cb97b4..4b3e4a8a2 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 3a424869b..3a043fd7d 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 d7c5c7d76..bbe0747e0 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 d1ad03121..d7d7a1dad 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 4fe1df930..a97f935f2 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 20d4fc260..2828ad4d0 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 82fa41c13..067990916 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 4e307962e..881ebedec 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 8516719fe..c03d8712f 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 c3cac8003..884f867c7 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 6d6ca0cf2..515daff2a 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 50ab80891..8db44ce4a 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 323616be9..f6c1eace8 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 c69f99b6a..3c781095f 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 f97edf645..d5b684163 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 a30f777fc..209dc12e0 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 54c08c69f..cc9615cf6 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 d5fdb9e5e..43894b7d6 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 a7a5677af..5c8b72ae3 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 c698db52f..dc3772d7b 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 0a5c2d796..5a3736d5e 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 493bc9dcd..adc0dabd9 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 e2bc1cf4e..485b3171b 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 14abaecd1..b8982f890 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 da318cafb..803c85b4f 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 d2af8f544..1c2655f88 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 b4427c2aa..96df91772 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 5820b99ba..086afab73 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 2cfc70f46..5b3f12ac6 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 a02d44208..47e0a32e2 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 5c2172ea9..c63e3d9b5 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 a3f9362b5..0af63b582 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 cc3dbb649..79cf183cd 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 5e5e3df19..2b4bfc4d8 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 e687c1446..bd553d20d 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 63a7baa2c..d4e90dd83 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 7487eb2fe..d2657e415 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 d6e3b9b1c..002aa206b 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 71217c8e6..3d6bcb3ae 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 cbe91a6fd..78492ae19 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 649ba85e9..b3c301049 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 6c8990c29..e9692a62c 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 7559bfa75..21c7c912f 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 71401381f..98af13f6e 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 82828c6c2..082e438d0 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 06436282a..9b4c7b6cf 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 42e01489e..45d570671 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 1dfcce935..06f9fd20e 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 6d0301692..6aee437dc 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 df5d418e0..2aa224b31 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 ab4f82bb1..34cb26dc3 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 8f68a5bd5..d4ed20575 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 e39c87088..79f7aff49 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 fb36c6d12..378757f09 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 e7a1d6a32..2c5102882 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 8a966f77b..822fc5b81 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 1e3b0940c..627549043 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 7479ca984..904dfd764 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 425f6a96b..cfbdd69e0 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 205c79f9d..0a02d13a1 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 6819cba18..d7b13f0c4 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 5d656eda2..fb4583063 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 cebdce1f8..9c9055ff5 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 04c7e6d7a..be68ea5ec 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 aad321781..a8389ce8b 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 e3d533a5f..99d47c9c3 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 d77defcbc..e74519b58 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 c114086a1..2538981e0 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 8532cc9f2..ae4a43a3b 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 5ca60e7f1..c7f7249a5 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 6d2c2fd8d..2801608bc 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)
-- 
GitLab