From d860c2b0b185a38a81e3d1d1464cc01d0a712ca5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joaquim=20Casals=20Bu=C3=B1uel?= <jcasals@iri.upc.edu>
Date: Mon, 28 Jan 2019 14:49:26 +0100
Subject: [PATCH] First step refactor, examples & hello_wolf working

---
 CMakeLists.txt                                |  33 ++++---
 hello_wolf/capture_range_bearing.h            |   2 +-
 hello_wolf/constraint_bearing.h               |   3 +-
 hello_wolf/constraint_range_bearing.h         |   3 +-
 hello_wolf/feature_range_bearing.cpp          |   1 -
 hello_wolf/feature_range_bearing.h            |   2 +-
 hello_wolf/hello_wolf.cpp                     |  19 +---
 hello_wolf/landmark_point_2D.h                |   2 +-
 hello_wolf/processor_range_bearing.cpp        |   3 +-
 hello_wolf/processor_range_bearing.h          |   3 +-
 hello_wolf/sensor_range_bearing.cpp           |   7 +-
 hello_wolf/sensor_range_bearing.h             |   4 +-
 include/base/IMU_tools.h                      |  13 +--
 include/base/association/association_nnls.h   |   3 +-
 include/base/association/association_node.h   |   5 +-
 include/base/association/association_solver.h |   2 +-
 include/base/association/association_tree.h   |   7 +-
 {src => include/base}/association/matrix.h    |   1 -
 include/base/capture/capture_GPS.h            |   3 +-
 include/base/capture/capture_GPS_fix.h        |   4 +-
 include/base/capture/capture_IMU.h            |   4 +-
 include/base/capture/capture_base.h           |  17 ++--
 include/base/capture/capture_buffer.h         |   5 +-
 include/base/capture/capture_image.h          |   6 +-
 include/base/capture/capture_laser_2D.h       |   5 +-
 include/base/capture/capture_motion.h         |   4 +-
 include/base/capture/capture_odom_2D.h        |   6 +-
 include/base/capture/capture_odom_3D.h        |   5 +-
 include/base/capture/capture_pose.h           |   8 +-
 include/base/capture/capture_velocity.h       |   2 +-
 include/base/capture/capture_void.h           |   3 +-
 .../capture/capture_wheel_joint_position.h    |   4 +-
 include/base/ceres_wrapper/ceres_manager.h    |   6 +-
 .../ceres_wrapper/cost_function_wrapper.h     |   6 +-
 .../create_numeric_diff_cost_function.h       |   6 +-
 .../local_parametrization_wrapper.h           |   4 +-
 include/base/ceres_wrapper/qr_manager.h       |   4 +-
 include/base/ceres_wrapper/solver_manager.h   |   6 +-
 include/base/constraint/constraint_AHP.h      |  13 +--
 include/base/constraint/constraint_GPS_2D.h   |   6 +-
 .../constraint_GPS_pseudorange_2D.h           |  14 +--
 .../constraint_GPS_pseudorange_3D.h           |   9 +-
 include/base/constraint/constraint_IMU.h      |  29 +-----
 include/base/constraint/constraint_analytic.h |   4 +-
 include/base/constraint/constraint_autodiff.h |  13 +--
 .../constraint_autodiff_distance_3D.h         |   2 +-
 .../constraint/constraint_autodiff_trifocal.h |  16 +---
 include/base/constraint/constraint_base.h     |  15 ++-
 .../constraint/constraint_block_absolute.h    |   5 +-
 .../base/constraint/constraint_container.h    |   7 +-
 .../base/constraint/constraint_corner_2D.h    |   6 +-
 .../base/constraint/constraint_diff_drive.h   |   8 +-
 include/base/constraint/constraint_epipolar.h |   3 +-
 include/base/constraint/constraint_fix_bias.h |  11 +--
 include/base/constraint/constraint_odom_2D.h  |   6 +-
 .../constraint/constraint_odom_2D_analytic.h  |   3 +-
 include/base/constraint/constraint_odom_3D.h  |   6 +-
 include/base/constraint/constraint_point_2D.h |   6 +-
 .../constraint/constraint_point_to_line_2D.h  |   6 +-
 include/base/constraint/constraint_pose_2D.h  |   7 +-
 include/base/constraint/constraint_pose_3D.h  |   7 +-
 .../constraint_quaternion_absolute.h          |   9 +-
 .../constraint_relative_2D_analytic.h         |   5 +-
 .../base}/diff_drive_tools.h                  |   5 +-
 .../base}/diff_drive_tools.hpp                |   0
 include/base/eigen_predicates.h               |   3 +-
 include/base/factory.h                        |   7 +-
 include/base/feature/feature_GPS_fix.h        |   4 +-
 .../base/feature/feature_GPS_pseudorange.h    |   3 +-
 include/base/feature/feature_IMU.h            |   9 +-
 include/base/feature/feature_base.h           |   7 +-
 include/base/feature/feature_corner_2D.h      |   3 +-
 .../base}/feature/feature_diff_drive.h        |   2 +-
 include/base/feature/feature_line_2D.h        |   3 +-
 include/base/feature/feature_match.h          |   3 +-
 include/base/feature/feature_motion.h         |   4 +-
 include/base/feature/feature_odom_2D.h        |   7 +-
 include/base/feature/feature_point_image.h    |   3 +-
 include/base/feature/feature_polyline_2D.h    |   2 +-
 include/base/feature/feature_pose.h           |   3 +-
 include/base/frame_base.h                     |  19 ++--
 include/base/hardware_base.h                  |   4 +-
 include/base/landmark/landmark_AHP.h          |   3 +-
 include/base/landmark/landmark_base.h         |  15 ++-
 include/base/landmark/landmark_container.h    |   6 +-
 include/base/landmark/landmark_corner_2D.h    |   3 +-
 include/base/landmark/landmark_line_2D.h      |   6 +-
 include/base/landmark/landmark_match.h        |   3 +-
 include/base/landmark/landmark_polyline_2D.h  |   2 +-
 include/base/local_parametrization_angle.h    |   4 +-
 include/base/local_parametrization_base.h     |   3 +-
 .../base/local_parametrization_homogeneous.h  |   3 +-
 .../local_parametrization_polyline_extreme.h  |   3 +-
 .../base/local_parametrization_quaternion.h   |   3 +-
 include/base/logging.h                        |   2 +-
 include/base/map_base.h                       |   4 +-
 include/base/motion_buffer.h                  |   6 +-
 include/base/node_base.h                      |   3 +-
 include/base/pinhole_tools.h                  |  20 +---
 include/base/problem.h                        |  16 +---
 include/base/processor/processor_GPS.h        |   5 +-
 include/base/processor/processor_IMU.h        |   7 +-
 include/base/processor/processor_base.h       |  14 ++-
 .../base/processor/processor_capture_holder.h |   6 +-
 .../base}/processor/processor_diff_drive.h    |   4 +-
 include/base/processor/processor_factory.h    |  13 ++-
 .../processor_frame_nearest_neighbor_filter.h |   4 +-
 include/base/processor/processor_logging.h    |   2 +-
 .../processor/processor_loopclosure_base.h    |   4 +-
 include/base/processor/processor_motion.h     |  18 +---
 include/base/processor/processor_odom_2D.h    |   9 +-
 include/base/processor/processor_odom_3D.h    |  12 +--
 .../base/processor/processor_params_image.h   |   4 +-
 include/base/processor/processor_tracker.h    |   4 +-
 .../processor/processor_tracker_feature.h     |  10 +-
 .../processor_tracker_feature_corner.h        |  17 ++--
 .../processor_tracker_feature_dummy.h         |   6 +-
 .../processor_tracker_feature_image.h         |  16 ++--
 .../processor/processor_tracker_landmark.h    |   9 +-
 .../processor_tracker_landmark_corner.h       |  18 ++--
 .../processor_tracker_landmark_dummy.h        |   2 +-
 .../processor_tracker_landmark_image.h        |  15 +--
 .../processor_tracker_landmark_polyline.h     |  21 ++---
 include/base/rotations.h                      |  14 +--
 include/base/sensor/sensor_GPS.h              |   5 +-
 include/base/sensor/sensor_GPS_fix.h          |   5 +-
 include/base/sensor/sensor_IMU.h              |   2 +-
 include/base/sensor/sensor_base.h             |  17 ++--
 include/base/sensor/sensor_camera.h           |   3 +-
 .../base}/sensor/sensor_diff_drive.h          |   4 +-
 include/base/sensor/sensor_factory.h          |   4 +-
 include/base/sensor/sensor_laser_2D.h         |   2 +-
 include/base/sensor/sensor_odom_2D.h          |   3 +-
 include/base/sensor/sensor_odom_3D.h          |   3 +-
 include/base/solver/solver_manager.h          |   6 +-
 .../solver_suitesparse/ccolamd_ordering.h     |   3 -
 .../solver_suitesparse/cost_function_base.h   |   3 +-
 .../solver_suitesparse/cost_function_sparse.h |  86 +-----------------
 .../cost_function_sparse_base.h               |   2 +-
 include/base/solver_suitesparse/qr_solver.h   |  12 +--
 include/base/solver_suitesparse/solver_QR.h   |   1 -
 .../base/solver_suitesparse/solver_manager.h  |   7 +-
 include/base/state_angle.h                    |   4 +-
 include/base/state_block.h                    |   8 +-
 include/base/state_homogeneous_3D.h           |   4 +-
 include/base/state_quaternion.h               |   4 +-
 include/base/time_stamp.h                     |   2 +-
 include/base/track_matrix.h                   |   4 +-
 include/base/trajectory_base.h                |   6 +-
 include/base/wolf.h                           |   8 +-
 serialization/cereal/io.h                     |   1 -
 ...serialization_local_parametrization_base.h |   3 +-
 ...zation_local_parametrization_homogeneous.h |   2 +-
 ...ization_local_parametrization_quaternion.h |   2 +-
 .../cereal/serialization_node_base.h          |   2 +-
 .../serialization_processor_odom2d_params.h   |   2 +-
 .../serialization_processor_odom3d_params.h   |   2 +-
 .../serialization_processor_params_base.h     |   2 +-
 .../serialization_sensor_intrinsic_base.h     |   2 +-
 .../serialization_sensor_odom2d_intrinsic.h   |   2 +-
 .../cereal/serialization_time_stamp.h         |   2 +-
 src/SE3.h                                     |  10 +-
 src/association/association_nnls.cpp          |   2 +-
 src/association/association_node.cpp          |   2 +-
 src/association/association_solver.cpp        |   2 +-
 src/association/association_tree.cpp          |   2 +-
 src/capture/capture_GPS.cpp                   |   4 +-
 src/capture/capture_GPS_fix.cpp               |   4 +-
 src/capture/capture_IMU.cpp                   |   9 +-
 src/capture/capture_base.cpp                  |   8 +-
 src/capture/capture_image.cpp                 |   3 +-
 src/capture/capture_laser_2D.cpp              |   2 +-
 src/capture/capture_motion.cpp                |   6 +-
 src/capture/capture_odom_2D.cpp               |   2 +-
 src/capture/capture_odom_3D.cpp               |   4 +-
 src/capture/capture_pose.cpp                  |   3 +-
 src/capture/capture_velocity.cpp              |   2 +-
 src/capture/capture_void.cpp                  |   3 +-
 src/capture/capture_wheel_joint_position.cpp  |   6 +-
 src/ceres_wrapper/ceres_manager.cpp           |  12 +--
 .../local_parametrization_wrapper.cpp         |   2 +-
 src/ceres_wrapper/qr_manager.cpp              |   1 -
 src/ceres_wrapper/solver_manager.cpp          |   8 +-
 src/constraint/constraint_analytic.cpp        |   4 +-
 src/constraint/constraint_base.cpp            |   6 +-
 {examples => src/examples}/.gitignore         |   0
 {examples => src/examples}/ACTIVESEARCH.yaml  |   0
 {examples => src/examples}/CMakeLists.txt     |   0
 {examples => src/examples}/Test_ORB.png       | Bin
 {examples => src/examples}/camera_params.yaml |   0
 .../examples}/camera_params_canonical.yaml    |   0
 .../examples}/camera_params_ueye.yaml         |   0
 .../camera_params_ueye_radial_dist.yaml       |   0
 .../examples}/camera_params_ueye_sim.yaml     |   0
 .../examples}/input_M3500b_toro.graph         |   0
 .../examples}/map_polyline_example.yaml       |   0
 .../examples}/processor_image_feature.yaml    |   0
 .../processor_image_vision_utils.yaml         |   0
 {examples => src/examples}/processor_imu.yaml |   0
 .../examples}/processor_imu_no_vote.yaml      |   0
 .../examples}/processor_imu_t1.yaml           |   0
 .../examples}/processor_imu_t6.yaml           |   0
 .../examples}/processor_odom_3D.yaml          |   0
 .../processor_tracker_feature_trifocal.yaml   |   0
 {examples => src/examples}/sensor_imu.yaml    |   0
 .../examples}/sensor_odom_3D.yaml             |   0
 .../examples}/sensor_odom_3D_HQ.yaml          |   0
 .../examples}/solver/test_SPQR.cpp            |   1 -
 .../examples}/solver/test_ccolamd.cpp         |   2 +-
 .../examples}/solver/test_ccolamd_blocks.cpp  |   5 -
 .../examples}/solver/test_iQR.cpp             |   7 --
 .../examples}/solver/test_iQR_wolf.cpp        |   8 --
 .../examples}/solver/test_iQR_wolf2.cpp       |  17 +---
 .../test_incremental_ccolamd_blocks.cpp       |   9 --
 .../examples}/solver/test_permutations.cpp    |   3 -
 .../examples}/test_2_lasers_offline.cpp       |  18 ++--
 .../test_analytic_odom_constraint.cpp         |   8 +-
 {examples => src/examples}/test_autodiff.cpp  |   5 +-
 .../examples}/test_capture_laser_2D.cpp       |   2 +-
 .../examples}/test_ceres_2_lasers.cpp         |  17 ++--
 .../test_ceres_2_lasers_polylines.cpp         |  17 ++--
 .../examples}/test_constraint_AHP.cpp         |  21 ++---
 .../examples}/test_constraint_imu.cpp         |  22 ++---
 .../examples}/test_constraint_odom_3D.cpp     |   2 +-
 .../examples}/test_diff_drive.cpp             |  10 +-
 .../examples}/test_eigen_quaternion.cpp       |   2 +-
 .../examples}/test_eigen_template.cpp         |   3 -
 .../examples}/test_faramotics_simulation.cpp  |  10 +-
 {examples => src/examples}/test_fcn_ptr.cpp   |   2 -
 {examples => src/examples}/test_image.cpp     |   8 +-
 {examples => src/examples}/test_imuDock.cpp   |  18 ++--
 .../examples}/test_imuDock_autoKFs.cpp        |  18 ++--
 .../examples}/test_imuPlateform_Offline.cpp   |  25 +++--
 .../examples}/test_imu_constrained0.cpp       |  27 +++---
 .../examples}/test_kf_callback.cpp            |  10 +-
 .../examples}/test_list_remove.cpp            |   2 -
 {examples => src/examples}/test_map_yaml.cpp  |  15 ++-
 .../examples}/test_matrix_prod.cpp            |   2 -
 {examples => src/examples}/test_mpu.cpp       |  15 ++-
 .../examples}/test_processor_imu.cpp          |  16 ++--
 .../test_processor_imu_jacobians.cpp          |  13 ++-
 .../examples}/test_processor_odom_3D.cpp      |  17 ++--
 .../test_processor_tracker_feature.cpp        |  12 +--
 .../test_processor_tracker_landmark.cpp       |  15 ++-
 .../test_processor_tracker_landmark_image.cpp |  23 ++---
 .../examples}/test_projection_points.cpp      |  43 +--------
 {examples => src/examples}/test_sh_ptr.cpp    |   8 --
 .../examples}/test_simple_AHP.cpp             |  38 ++------
 .../examples}/test_sort_keyframes.cpp         |  10 +-
 .../examples}/test_sparsification.cpp         |   9 +-
 .../examples}/test_state_quaternion.cpp       |   8 +-
 .../examples}/test_tracker_ORB.cpp            |   3 +-
 .../examples}/test_virtual_hierarchy.cpp      |   0
 .../examples}/test_wolf_autodiffwrapper.cpp   |   6 +-
 .../examples}/test_wolf_factories.cpp         |  22 ++---
 .../examples}/test_wolf_imported_graph.cpp    |   8 +-
 .../examples}/test_wolf_logging.cpp           |   4 +-
 .../examples}/test_wolf_prunning.cpp          |   9 +-
 {examples => src/examples}/test_wolf_root.cpp |   2 +-
 {examples => src/examples}/test_wolf_tree.cpp |   0
 {examples => src/examples}/test_yaml.cpp      |   8 +-
 .../examples}/test_yaml_conversions.cpp       |   4 +-
 .../examples}/vision_utils_active_search.yaml |   0
 src/feature/feature_GPS_fix.cpp               |   2 +-
 src/feature/feature_GPS_pseudorange.cpp       |   2 +-
 src/feature/feature_IMU.cpp                   |   2 +-
 src/feature/feature_base.cpp                  |   6 +-
 src/feature/feature_corner_2D.cpp             |   2 +-
 src/feature/feature_diff_drive.cpp            |   2 +-
 src/feature/feature_line_2D.cpp               |   2 +-
 src/feature/feature_odom_2D.cpp               |   2 +-
 src/feature/feature_point_image.cpp           |   2 +-
 src/feature/feature_polyline_2D.cpp           |   2 +-
 src/feature/feature_pose.cpp                  |   3 +-
 src/frame_base.cpp                            |  18 ++--
 src/hardware_base.cpp                         |   5 +-
 src/landmark/landmark_AHP.cpp                 |   6 +-
 src/landmark/landmark_base.cpp                |  12 +--
 src/landmark/landmark_container.cpp           |   4 +-
 src/landmark/landmark_corner_2D.cpp           |   3 +-
 src/landmark/landmark_line_2D.cpp             |   4 +-
 src/landmark/landmark_point_3D.h              |   3 +-
 src/landmark/landmark_polyline_2D.cpp         |  18 ++--
 src/local_parametrization_base.cpp            |   3 +-
 src/local_parametrization_homogeneous.cpp     |   4 +-
 ...local_parametrization_polyline_extreme.cpp |   6 +-
 src/local_parametrization_quaternion.cpp      |   7 +-
 src/map_base.cpp                              |   9 +-
 src/motion_buffer.cpp                         |   4 +-
 src/node_base.cpp                             |   2 +-
 src/problem.cpp                               |  37 +++-----
 src/processor/processor_GPS.cpp               |   9 +-
 src/processor/processor_IMU.cpp               |   9 +-
 src/processor/processor_base.cpp              |  12 +--
 src/processor/processor_capture_holder.cpp    |   5 +-
 src/processor/processor_diff_drive.cpp        |  16 ++--
 ...rocessor_frame_nearest_neighbor_filter.cpp |   2 +-
 src/processor/processor_loopclosure_base.cpp  |   2 +-
 src/processor/processor_motion.cpp            |   8 +-
 src/processor/processor_odom_2D.cpp           |   7 +-
 src/processor/processor_odom_3D.cpp           |   7 +-
 src/processor/processor_tracker.cpp           |   6 +-
 src/processor/processor_tracker_feature.cpp   |   2 +-
 .../processor_tracker_feature_corner.cpp      |   4 +-
 .../processor_tracker_feature_dummy.cpp       |   4 +-
 .../processor_tracker_feature_image.cpp       |   5 +-
 .../processor_tracker_feature_trifocal.cpp    |  10 +-
 .../processor_tracker_feature_trifocal.h      |   5 +-
 src/processor/processor_tracker_landmark.cpp  |   5 +-
 .../processor_tracker_landmark_corner.cpp     |   6 +-
 .../processor_tracker_landmark_dummy.cpp      |   6 +-
 .../processor_tracker_landmark_image.cpp      |  34 +++----
 .../processor_tracker_landmark_polyline.cpp   |   7 +-
 src/sensor/sensor_GPS.cpp                     |  10 +-
 src/sensor/sensor_GPS_fix.cpp                 |  10 +-
 src/sensor/sensor_IMU.cpp                     |  10 +-
 src/sensor/sensor_base.cpp                    |  11 +--
 src/sensor/sensor_camera.cpp                  |  14 ++-
 src/sensor/sensor_diff_drive.cpp              |  12 +--
 src/sensor/sensor_laser_2D.cpp                |  10 +-
 src/sensor/sensor_odom_2D.cpp                 |  10 +-
 src/sensor/sensor_odom_3D.cpp                 |  10 +-
 src/solver/solver_manager.cpp                 |   8 +-
 src/solver_suitesparse/solver_manager.cpp     |   2 +-
 src/state_block.cpp                           |   2 +-
 src/time_stamp.cpp                            |   4 +-
 src/track_matrix.cpp                          |   2 +-
 src/trajectory_base.cpp                       |   6 +-
 src/yaml/processor_IMU_yaml.cpp               |   6 +-
 src/yaml/processor_image_yaml.cpp             |   8 +-
 src/yaml/processor_odom_3D_yaml.cpp           |   6 +-
 ...rocessor_tracker_feature_trifocal_yaml.cpp |   8 +-
 src/yaml/sensor_IMU_yaml.cpp                  |   6 +-
 src/yaml/sensor_camera_yaml.cpp               |  10 +-
 src/yaml/sensor_laser_2D_yaml.cpp             |  10 +-
 src/yaml/sensor_odom_3D_yaml.cpp              |   6 +-
 test/gtest_IMU.cpp                            |  66 ++------------
 test/gtest_IMU_tools.cpp                      |   5 +-
 test/gtest_capture_base.cpp                   |   5 +-
 test/gtest_ceres_manager.cpp                  |  25 +++--
 test/gtest_constraint_IMU.cpp                 |  20 ++--
 test/gtest_constraint_absolute.cpp            |  10 +-
 test/gtest_constraint_autodiff.cpp            |  13 ++-
 .../gtest_constraint_autodiff_distance_3D.cpp |  10 +-
 test/gtest_constraint_autodiff_trifocal.cpp   |  10 +-
 test/gtest_constraint_odom_3D.cpp             |   8 +-
 test/gtest_constraint_pose_2D.cpp             |   9 +-
 test/gtest_constraint_pose_3D.cpp             |  11 +--
 test/gtest_constraint_sparse.cpp              |   1 -
 test/gtest_eigen_predicates.cpp               |   2 +-
 test/gtest_feature_IMU.cpp                    |  16 ++--
 test/gtest_feature_base.cpp                   |   3 +-
 test/gtest_frame_base.cpp                     |  24 ++---
 test/gtest_local_param.cpp                    |  13 +--
 test/gtest_make_posdef.cpp                    |   2 +-
 test/gtest_motion_buffer.cpp                  |   8 +-
 test/gtest_odom_2D.cpp                        |  20 ++--
 test/gtest_odom_3D.cpp                        |  19 +---
 test/gtest_pack_KF_buffer.cpp                 |  10 +-
 test/gtest_pinhole.cpp                        |   4 +-
 test/gtest_problem.cpp                        |  20 ++--
 test/gtest_processor_IMU.cpp                  |  19 ++--
 test/gtest_processor_IMU_jacobians.cpp        |  16 ++--
 test/gtest_processor_base.cpp                 |  11 +--
 ...essor_frame_nearest_neighbor_filter_2D.cpp |   7 +-
 test/gtest_processor_motion.cpp               |  12 +--
 ...est_processor_tracker_feature_trifocal.cpp |  12 +--
 test/gtest_rotation.cpp                       |  10 +-
 test/gtest_sensor_base.cpp                    |   3 +-
 test/gtest_shared_from_this.cpp               |   5 +-
 test/gtest_solver_manager.cpp                 |  20 ++--
 test/gtest_time_stamp.cpp                     |   2 +-
 test/gtest_track_matrix.cpp                   |   2 +-
 test/gtest_trajectory.cpp                     |  10 +-
 test/processor_IMU_UnitTester.h               |  10 +-
 .../gtest_serialization_eigen_geometry.cpp    |   2 +-
 .../gtest_serialization_eigen_sparse.cpp      |   2 +-
 ...st_serialization_local_parametrization.cpp |   1 -
 test/utils_gtest.h                            |   3 -
 wolf_scripts/include_refactor.sh              |  39 ++++----
 380 files changed, 1019 insertions(+), 1838 deletions(-)
 rename {src => include/base}/association/matrix.h (99%)
 rename {src/sensor => include/base}/diff_drive_tools.h (99%)
 rename {src/sensor => include/base}/diff_drive_tools.hpp (100%)
 rename {src => include/base}/feature/feature_diff_drive.h (95%)
 rename {src => include/base}/processor/processor_diff_drive.h (98%)
 rename {src => include/base}/sensor/sensor_diff_drive.h (97%)
 rename {examples => src/examples}/.gitignore (100%)
 rename {examples => src/examples}/ACTIVESEARCH.yaml (100%)
 rename {examples => src/examples}/CMakeLists.txt (100%)
 rename {examples => src/examples}/Test_ORB.png (100%)
 rename {examples => src/examples}/camera_params.yaml (100%)
 rename {examples => src/examples}/camera_params_canonical.yaml (100%)
 rename {examples => src/examples}/camera_params_ueye.yaml (100%)
 rename {examples => src/examples}/camera_params_ueye_radial_dist.yaml (100%)
 rename {examples => src/examples}/camera_params_ueye_sim.yaml (100%)
 rename {examples => src/examples}/input_M3500b_toro.graph (100%)
 rename {examples => src/examples}/map_polyline_example.yaml (100%)
 rename {examples => src/examples}/processor_image_feature.yaml (100%)
 rename {examples => src/examples}/processor_image_vision_utils.yaml (100%)
 rename {examples => src/examples}/processor_imu.yaml (100%)
 rename {examples => src/examples}/processor_imu_no_vote.yaml (100%)
 rename {examples => src/examples}/processor_imu_t1.yaml (100%)
 rename {examples => src/examples}/processor_imu_t6.yaml (100%)
 rename {examples => src/examples}/processor_odom_3D.yaml (100%)
 rename {examples => src/examples}/processor_tracker_feature_trifocal.yaml (100%)
 rename {examples => src/examples}/sensor_imu.yaml (100%)
 rename {examples => src/examples}/sensor_odom_3D.yaml (100%)
 rename {examples => src/examples}/sensor_odom_3D_HQ.yaml (100%)
 rename {examples => src/examples}/solver/test_SPQR.cpp (99%)
 rename {examples => src/examples}/solver/test_ccolamd.cpp (99%)
 rename {examples => src/examples}/solver/test_ccolamd_blocks.cpp (99%)
 rename {examples => src/examples}/solver/test_iQR.cpp (99%)
 rename {examples => src/examples}/solver/test_iQR_wolf.cpp (99%)
 rename {examples => src/examples}/solver/test_iQR_wolf2.cpp (99%)
 rename {examples => src/examples}/solver/test_incremental_ccolamd_blocks.cpp (99%)
 rename {examples => src/examples}/solver/test_permutations.cpp (99%)
 rename {examples => src/examples}/test_2_lasers_offline.cpp (97%)
 rename {examples => src/examples}/test_analytic_odom_constraint.cpp (99%)
 rename {examples => src/examples}/test_autodiff.cpp (99%)
 rename {examples => src/examples}/test_capture_laser_2D.cpp (99%)
 rename {examples => src/examples}/test_ceres_2_lasers.cpp (98%)
 rename {examples => src/examples}/test_ceres_2_lasers_polylines.cpp (98%)
 rename {examples => src/examples}/test_constraint_AHP.cpp (96%)
 rename {examples => src/examples}/test_constraint_imu.cpp (97%)
 rename {examples => src/examples}/test_constraint_odom_3D.cpp (73%)
 rename {examples => src/examples}/test_diff_drive.cpp (97%)
 rename {examples => src/examples}/test_eigen_quaternion.cpp (97%)
 rename {examples => src/examples}/test_eigen_template.cpp (99%)
 rename {examples => src/examples}/test_faramotics_simulation.cpp (98%)
 rename {examples => src/examples}/test_fcn_ptr.cpp (99%)
 rename {examples => src/examples}/test_image.cpp (97%)
 rename {examples => src/examples}/test_imuDock.cpp (97%)
 rename {examples => src/examples}/test_imuDock_autoKFs.cpp (97%)
 rename {examples => src/examples}/test_imuPlateform_Offline.cpp (96%)
 rename {examples => src/examples}/test_imu_constrained0.cpp (97%)
 rename {examples => src/examples}/test_kf_callback.cpp (93%)
 rename {examples => src/examples}/test_list_remove.cpp (99%)
 rename {examples => src/examples}/test_map_yaml.cpp (94%)
 rename {examples => src/examples}/test_matrix_prod.cpp (99%)
 rename {examples => src/examples}/test_mpu.cpp (97%)
 rename {examples => src/examples}/test_processor_imu.cpp (97%)
 rename {examples => src/examples}/test_processor_imu_jacobians.cpp (99%)
 rename {examples => src/examples}/test_processor_odom_3D.cpp (91%)
 rename {examples => src/examples}/test_processor_tracker_feature.cpp (87%)
 rename {examples => src/examples}/test_processor_tracker_landmark.cpp (93%)
 rename {examples => src/examples}/test_processor_tracker_landmark_image.cpp (96%)
 rename {examples => src/examples}/test_projection_points.cpp (99%)
 rename {examples => src/examples}/test_sh_ptr.cpp (99%)
 rename {examples => src/examples}/test_simple_AHP.cpp (96%)
 rename {examples => src/examples}/test_sort_keyframes.cpp (95%)
 rename {examples => src/examples}/test_sparsification.cpp (98%)
 rename {examples => src/examples}/test_state_quaternion.cpp (90%)
 rename {examples => src/examples}/test_tracker_ORB.cpp (99%)
 rename {examples => src/examples}/test_virtual_hierarchy.cpp (100%)
 rename {examples => src/examples}/test_wolf_autodiffwrapper.cpp (99%)
 rename {examples => src/examples}/test_wolf_factories.cpp (92%)
 rename {examples => src/examples}/test_wolf_imported_graph.cpp (99%)
 rename {examples => src/examples}/test_wolf_logging.cpp (87%)
 rename {examples => src/examples}/test_wolf_prunning.cpp (99%)
 rename {examples => src/examples}/test_wolf_root.cpp (92%)
 rename {examples => src/examples}/test_wolf_tree.cpp (100%)
 rename {examples => src/examples}/test_yaml.cpp (98%)
 rename {examples => src/examples}/test_yaml_conversions.cpp (97%)
 rename {examples => src/examples}/vision_utils_active_search.yaml (100%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2fb76c6fa..b904e0722 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -197,6 +197,8 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
 ENDIF()
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
+message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+include_directories("${PROJECT_BINARY_DIR}/conf")
 # include spdlog (logging library)
 FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
 IF (SPDLOG_INCLUDE_DIR)
@@ -207,7 +209,8 @@ ELSE (SPDLOG_INCLUDE_DIR)
 ENDIF (SPDLOG_INCLUDE_DIR)  
   
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
+include_directories("include")
+include_directories("templinks")
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
@@ -283,8 +286,6 @@ include/base/constraint/constraint_pose_2D.h
 include/base/constraint/constraint_pose_3D.h
 include/base/constraint/constraint_quaternion_absolute.h
 include/base/constraint/constraint_relative_2D_analytic.h
-src/sensor/diff_drive_tools.hpp
-    temp/diff_drive_tools.hpp
 include/base/feature/feature_corner_2D.h
 include/base/feature/feature_GPS_fix.h
 include/base/feature/feature_GPS_pseudorange.h
@@ -460,7 +461,6 @@ src/processor/processor_base.cpp
 src/processor/processor_loopclosure_base.cpp
 src/processor/processor_motion.cpp
 src/processor/processor_tracker.cpp
-examples/test_processor_tracker_feature.cpp
 src/sensor/sensor_base.cpp
 src/state_block.cpp
 src/time_stamp.cpp
@@ -471,7 +471,7 @@ src/trajectory_base.cpp
 SET(SRCS_BASE
 src/capture/capture_motion.cpp
 src/processor/processor_capture_holder.cpp
-examples/test_processor_tracker_landmark.cpp
+# examples/test_processor_tracker_landmark.cpp
     )
 
 SET(SRCS
@@ -506,7 +506,7 @@ src/processor/processor_frame_nearest_neighbor_filter.cpp
 src/processor/processor_diff_drive.cpp
 src/processor/processor_IMU.cpp
 src/processor/processor_odom_2D.cpp
-examples/test_processor_odom_3D.cpp
+src/processor/processor_odom_3D.cpp
 src/processor/processor_tracker_feature_dummy.cpp
 src/processor/processor_tracker_landmark_dummy.cpp
     )
@@ -560,11 +560,14 @@ include/base/processor/processor_tracker_landmark_corner.h
 include/base/processor/processor_tracker_landmark_polyline.h
         )
     SET(SRCS ${SRCS}
-examples/test_capture_laser_2D.cpp
+# examples/test_capture_laser_2D.cpp
 src/sensor/sensor_laser_2D.cpp
+src/processor/processor_tracker_feature.cpp
 src/processor/processor_tracker_feature_corner.cpp
 src/processor/processor_tracker_landmark_corner.cpp
 src/processor/processor_tracker_landmark_polyline.cpp
+src/processor/processor_tracker_landmark.cpp
+src/processor/processor_tracker_landmark_dummy.cpp
         )
 ENDIF(laser_scan_utils_FOUND)
 
@@ -600,7 +603,7 @@ src/capture/capture_image.cpp
 src/feature/feature_point_image.cpp
 src/landmark/landmark_AHP.cpp
 src/processor/processor_tracker_feature_image.cpp
-examples/test_processor_tracker_landmark_image.cpp
+# examples/test_processor_tracker_landmark_image.cpp
         )
     SET(SRCS_LANDMARK ${SRCS_LANDMARK}
 src/landmark/landmark_point_3D.cpp
@@ -611,9 +614,7 @@ src/processor/processor_tracker_feature_trifocal.cpp
 ENDIF(vision_utils_FOUND)
 
 #SUBDIRECTORIES
-
-ADD_SUBDIRECTORY(hello_wolf)
-
+add_subdirectory(hello_wolf)
 IF (cereal_FOUND)
   ADD_SUBDIRECTORY(serialization/cereal)
 ENDIF(cereal_FOUND)
@@ -669,9 +670,8 @@ ADD_LIBRARY(${PROJECT_NAME}
             ${SRCS_SOLVER}
             ${SRCS_WRAPPER}
             )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core ${CMAKE_THREAD_LIBS_INIT})
 
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
 #Link the created libraries
 #=============================================================
 IF (Ceres_FOUND)
@@ -700,6 +700,13 @@ ENDIF (YAMLCPP_FOUND)
 IF (GLOG_FOUND)
     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
 ENDIF (GLOG_FOUND)
+IF (GLOG_FOUND)
+    IF(BUILD_TESTS)
+        MESSAGE("Building tests.")
+        add_subdirectory(test)
+    ENDIF(BUILD_TESTS)
+ENDIF (GLOG_FOUND)
+
 #-END_SRC --------------------------------------------------------------------------------------------------------------------------------
 FIND_PACKAGE(Doxygen)
 
diff --git a/hello_wolf/capture_range_bearing.h b/hello_wolf/capture_range_bearing.h
index 6e18876cb..5a7e5a771 100644
--- a/hello_wolf/capture_range_bearing.h
+++ b/hello_wolf/capture_range_bearing.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_CAPTURE_RANGE_BEARING_H_
 #define HELLO_WOLF_CAPTURE_RANGE_BEARING_H_
 
-#include "capture_base.h"
+#include "base/capture/capture_base.h"
 
 namespace wolf
 {
diff --git a/hello_wolf/constraint_bearing.h b/hello_wolf/constraint_bearing.h
index 1ce1c7d78..7849f17d4 100644
--- a/hello_wolf/constraint_bearing.h
+++ b/hello_wolf/constraint_bearing.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_CONSTRAINT_BEARING_H_
 #define HELLO_WOLF_CONSTRAINT_BEARING_H_
 
-#include "constraint_autodiff.h"
+#include "base/constraint/constraint_autodiff.h"
 
 namespace wolf
 {
@@ -46,7 +46,6 @@ class ConstraintBearing : public ConstraintAutodiff<ConstraintBearing, 1, 2, 1,
 
 } /* namespace wolf */
 
-
 ////////////////   IMPLEMENTATION   //////////////////////////////////////////
 
 namespace wolf
diff --git a/hello_wolf/constraint_range_bearing.h b/hello_wolf/constraint_range_bearing.h
index 89b513786..cf7af72a4 100644
--- a/hello_wolf/constraint_range_bearing.h
+++ b/hello_wolf/constraint_range_bearing.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
 #define HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
 
-#include "constraint_autodiff.h"
+#include "base/constraint/constraint_autodiff.h"
 
 namespace wolf
 {
@@ -60,7 +60,6 @@ class ConstraintRangeBearing : public ConstraintAutodiff<ConstraintRangeBearing,
 
 } /* namespace wolf */
 
-
 //////////////   IMPLEMENTATION   //////////////////////////////////
 
 namespace wolf
diff --git a/hello_wolf/feature_range_bearing.cpp b/hello_wolf/feature_range_bearing.cpp
index ce35c61f3..3be936567 100644
--- a/hello_wolf/feature_range_bearing.cpp
+++ b/hello_wolf/feature_range_bearing.cpp
@@ -16,7 +16,6 @@ FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXs& _measurement, co
     //
 }
 
-
 FeatureRangeBearing::~FeatureRangeBearing()
 {
     //
diff --git a/hello_wolf/feature_range_bearing.h b/hello_wolf/feature_range_bearing.h
index e3d30bba2..9da7215a5 100644
--- a/hello_wolf/feature_range_bearing.h
+++ b/hello_wolf/feature_range_bearing.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_FEATURE_RANGE_BEARING_H_
 #define HELLO_WOLF_FEATURE_RANGE_BEARING_H_
 
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 namespace wolf
 {
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 51ef873b3..646f1c2d6 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -12,11 +12,10 @@
  *      \author: jsola
  */
 
+#include "base/wolf.h"
 
-#include "wolf.h"
-
-#include "sensor_odom_2D.h"
-#include "processor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
 #include "sensor_range_bearing.h"
 #include "processor_range_bearing.h"
 #include "capture_range_bearing.h"
@@ -24,24 +23,21 @@
 #include "constraint_range_bearing.h"
 #include "landmark_point_2D.h"
 
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 int main()
  {
     /*
      * ============= PROBLEM DEFINITION ==================
      *
-     *
      * We have a planar robot with a range-and-bearing sensor 'S' mounted at its front-left corner, looking forward:
      *
      *              ^ Y
      *              |
      *     ------------------S->        sensor at location (1,1) and orientation 0 degrees, that is, at pose (1,1,0).
      *     |        |        |
-     *     |        |        |
      *     |        +--------|--> X     robot axes X, Y
      *     |                 |
-     *     |                 |
      *     -------------------
      *
      * The robot performs a straight trajectory with 3 keyframes 'KF', and observes 3 landmarks 'L'.
@@ -62,7 +58,6 @@ int main()
      *     KF1->---KF2->---KF3->                KEYFRAMES -- robot poses
      *   (0,0,0) (1,0,0) (2,0,0)                keyframe poses in world frame
      *      |
-     *      |
      *      * prior                             Initial robot pose in world frame
      *    (0,0,0)
      *
@@ -82,7 +77,6 @@ int main()
      *   - Observations have ranges 1 or sqrt(2)
      *   - Observations have bearings pi/2 or 3pi/4
      *
-     *
      * The robot starts at (0,0,0) with a map with no previously known landmarks.
      * At each keyframe, it does:
      *   - Create a motion factor to the previous keyframe
@@ -143,7 +137,6 @@ int main()
     params_rb->time_tolerance               = 0.01;
     ProcessorBasePtr processor_rb           = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
 
-
     // SELF CALIBRATION ===================================================
 
     // NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
@@ -154,7 +147,6 @@ int main()
     // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
     // sensor_rb->getPPtr()->unfix();
 
-
     // CONFIGURE ==========================================================
 
     // Motion data
@@ -165,7 +157,6 @@ int main()
     VectorXi ids;
     VectorXs ranges, bearings;
 
-
     // SET OF EVENTS =======================================================
     std::cout << std::endl;
     WOLF_TRACE("======== BUILD PROBLEM =======")
@@ -219,7 +210,6 @@ int main()
     sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2), L3 : (3,2)
     problem->print(1,0,1,0);
 
-
     // SOLVE ================================================================
 
     // SOLVE with exact initial guess
@@ -283,7 +273,6 @@ int main()
      *
      */
 
-
     /*
      * ============= DETAILED DESCRIPTION OF THE PRINTED RESULT ==================
      *
diff --git a/hello_wolf/landmark_point_2D.h b/hello_wolf/landmark_point_2D.h
index cb588f8eb..1bd845763 100644
--- a/hello_wolf/landmark_point_2D.h
+++ b/hello_wolf/landmark_point_2D.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_LANDMARK_POINT_2D_H_
 #define HELLO_WOLF_LANDMARK_POINT_2D_H_
 
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
 
 namespace wolf
 {
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 4f8e0dabf..670a61ea6 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -161,9 +161,8 @@ Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const
 
 } /* namespace wolf */
 
-
 // Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf
 {
 WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing)
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 5d3d2a8dd..6e8c77cd6 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
 #define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
 
-#include "processor_base.h"
+#include "base/processor/processor_base.h"
 #include "sensor_range_bearing.h"
 #include "Eigen/Geometry"
 
@@ -24,7 +24,6 @@ struct ProcessorParamsRangeBearing : public ProcessorParamsBase
         // We do not need special parameters, but in case you need they should be defined here.
 };
 
-
 using namespace Eigen;
 WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
 
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index d7de5aa0b..04d704664 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 "state_angle.h"
+#include "base/state_angle.h"
 
 namespace wolf
 {
@@ -28,7 +28,6 @@ SensorRangeBearing::~SensorRangeBearing()
     //
 }
 
-
 SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
         const Eigen::VectorXs& _extrinsics, //
         const IntrinsicsBasePtr _intrinsics)
@@ -45,12 +44,10 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
 
 } /* namespace wolf */
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf
 {
 WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
 } // namespace wolf
 
-
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 3e4e7c744..01ac8d93e 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -8,7 +8,7 @@
 #ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
 #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
 
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 namespace wolf
 {
@@ -20,8 +20,6 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
         Scalar noise_bearing_degrees_std    = 0.5;
 };
 
-
-
 WOLF_PTR_TYPEDEFS(SensorRangeBearing)
 
 class SensorRangeBearing : public SensorBase
diff --git a/include/base/IMU_tools.h b/include/base/IMU_tools.h
index 6d2312235..eecad244c 100644
--- a/include/base/IMU_tools.h
+++ b/include/base/IMU_tools.h
@@ -8,9 +8,8 @@
 #ifndef IMU_TOOLS_H_
 #define IMU_TOOLS_H_
 
-
-#include "wolf.h"
-#include "rotations.h"
+#include "base/wolf.h"
+#include "base/rotations.h"
 
 /*
  * Most functions in this file are explained in the document:
@@ -41,8 +40,6 @@
  *   - body2delta: construct a delta from body magnitudes of linAcc and angVel
  */
 
-
-
 namespace wolf 
 {
 namespace imu {
@@ -110,7 +107,6 @@ inline void inverse(const MatrixBase<D1>& d,
     inverse(dp, dq, dv, dt, idp, idq, idv);
 }
 
-
 template<typename D, class T>
 inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
                                                  T dt)
@@ -247,7 +243,6 @@ inline void between(const MatrixBase<D1>& d1,
     between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
 }
 
-
 template<typename D1, typename D2, class T>
 inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
                                                   const MatrixBase<D2>& d2,
@@ -445,7 +440,6 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const
     J_do_dq2    =   jac_SO3_right_inv(diff_o);
 }
 
-
 template<typename D1, typename D2, typename D3>
 inline void diff(const MatrixBase<D1>& d1,
                  const MatrixBase<D2>& d2,
@@ -511,7 +505,6 @@ inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
     return ret;
 }
 
-
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
 inline void body2delta(const MatrixBase<D1>& a,
                        const MatrixBase<D2>& w,
@@ -611,8 +604,6 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons
     return  data;
 }
 
-
-
 } // namespace imu
 } // namespace wolf
 
diff --git a/include/base/association/association_nnls.h b/include/base/association/association_nnls.h
index 0883a7292..6563a1f66 100644
--- a/include/base/association/association_nnls.h
+++ b/include/base/association/association_nnls.h
@@ -7,8 +7,7 @@
 #include <vector>
 
 //pipol tracker
-#include "association_solver.h"
-
+#include "base/association/association_solver.h"
 
 namespace wolf
 {
diff --git a/include/base/association/association_node.h b/include/base/association/association_node.h
index cc4422e81..21a10d480 100644
--- a/include/base/association/association_node.h
+++ b/include/base/association/association_node.h
@@ -1,5 +1,4 @@
-
-
+    
 #ifndef association_node_H
 #define association_node_H
 
@@ -10,7 +9,7 @@
 #include <algorithm> //find()
 
 //pipol tracker
-#include "matrix.h"
+#include "base/association/matrix.h"
 
 //constants
 const double PROB_ZERO_ = 1e-3;
diff --git a/include/base/association/association_solver.h b/include/base/association/association_solver.h
index 24efc159b..6ac986e07 100644
--- a/include/base/association/association_solver.h
+++ b/include/base/association/association_solver.h
@@ -7,7 +7,7 @@
 #include <vector>
 
 //matrix class
-#include "matrix.h"
+#include "base/association/matrix.h"
 
 namespace wolf
 {
diff --git a/include/base/association/association_tree.h b/include/base/association/association_tree.h
index 29ae34a2c..b6566a604 100644
--- a/include/base/association/association_tree.h
+++ b/include/base/association/association_tree.h
@@ -9,12 +9,11 @@
 //#include <memory>
 
 //pipol tracker
-#include "matrix.h"
-#include "association_solver.h"
-#include "association_node.h"
+#include "base/association/matrix.h"
+#include "base/association/association_solver.h"
+#include "base/association/association_node.h"
 #include <map>
 
-
 namespace wolf
 {
 
diff --git a/src/association/matrix.h b/include/base/association/matrix.h
similarity index 99%
rename from src/association/matrix.h
rename to include/base/association/matrix.h
index 10031b131..c76bba51e 100644
--- a/src/association/matrix.h
+++ b/include/base/association/matrix.h
@@ -7,7 +7,6 @@
 #include <vector>
 #include <assert.h> //assert
 
-
 template <typename T>
 class Matrixx
 {
diff --git a/include/base/capture/capture_GPS.h b/include/base/capture/capture_GPS.h
index 946619527..5f5088bd6 100644
--- a/include/base/capture/capture_GPS.h
+++ b/include/base/capture/capture_GPS.h
@@ -1,10 +1,9 @@
 #ifndef CAPTURE_GPS_H_
 #define CAPTURE_GPS_H_
 
-
 // Wolf includes
 #include "raw_gps_utils/satellites_obs.h"
-#include "capture_base.h"
+#include "base/capture/capture_base.h"
 
 namespace wolf {
     
diff --git a/include/base/capture/capture_GPS_fix.h b/include/base/capture/capture_GPS_fix.h
index 6f65b733b..8cb4afbf4 100644
--- a/include/base/capture/capture_GPS_fix.h
+++ b/include/base/capture/capture_GPS_fix.h
@@ -2,8 +2,8 @@
 #define CAPTURE_GPS_FIX_H_
 
 //Wolf includes
-#include "feature_GPS_fix.h"
-#include "capture_base.h"
+#include "base/feature/feature_GPS_fix.h"
+#include "base/capture/capture_base.h"
 
 //std includes
 //
diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h
index 01c1f5aab..6e2de9d9d 100644
--- a/include/base/capture/capture_IMU.h
+++ b/include/base/capture/capture_IMU.h
@@ -2,8 +2,8 @@
 #define CAPTURE_IMU_H
 
 //Wolf includes
-#include "IMU_tools.h"
-#include "capture_motion.h"
+#include "base/IMU_tools.h"
+#include "base/capture/capture_motion.h"
 
 namespace wolf {
     
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index 9da734e64..293ab9d03 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -8,9 +8,9 @@ class FeatureBase;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
 
 //std includes
 
@@ -109,10 +109,10 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
 
 }
 
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "feature_base.h"
-#include "state_block.h"
+#include "base/sensor/sensor_base.h"
+#include "base/frame_base.h"
+#include "base/feature/feature_base.h"
+#include "base/state_block.h"
 
 namespace wolf{
 
@@ -148,7 +148,6 @@ inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _
     state_block_vec_[_i] = _sb_ptr;
 }
 
-
 inline StateBlockPtr CaptureBase::getSensorPPtr() const
 {
     return getStateBlockPtr(0);
@@ -164,7 +163,6 @@ inline StateBlockPtr CaptureBase::getSensorIntrinsicPtr() const
     return getStateBlockPtr(2);
 }
 
-
 inline unsigned int CaptureBase::id()
 {
     return capture_id_;
@@ -195,7 +193,6 @@ inline ConstraintBaseList& CaptureBase::getConstrainedByList()
     return constrained_by_list_;
 }
 
-
 inline TimeStamp CaptureBase::getTimeStamp() const
 {
     return time_stamp_;
diff --git a/include/base/capture/capture_buffer.h b/include/base/capture/capture_buffer.h
index 13cb1b9d0..434716dcf 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 "wolf.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/time_stamp.h"
 
 #include <list>
 #include <algorithm>
@@ -79,7 +79,6 @@ public:
   std::list<CaptureBasePtr> container_;
 };
 
-
 CaptureBuffer::CaptureBuffer(const Scalar _buffer_dt, const int _max_capture) :
   max_capture_(_max_capture), buffer_dt_(_buffer_dt)
 {
diff --git a/include/base/capture/capture_image.h b/include/base/capture/capture_image.h
index ca10a873f..4e9771cf0 100644
--- a/include/base/capture/capture_image.h
+++ b/include/base/capture/capture_image.h
@@ -2,9 +2,9 @@
 #define CAPTURE_IMAGE_H
 
 //Wolf includes
-#include "capture_base.h"
-#include "feature_point_image.h"
-#include "sensor_camera.h"
+#include "base/capture/capture_base.h"
+#include "base/feature/feature_point_image.h"
+#include "base/sensor/sensor_camera.h"
 
 // Vision Utils includes
 #include "vision_utils/vision_utils.h"
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
index f1cbf9eae..9abd6bf4b 100644
--- a/include/base/capture/capture_laser_2D.h
+++ b/include/base/capture/capture_laser_2D.h
@@ -8,8 +8,8 @@ class SensorLaser2D;
 }
 
 //wolf includes
-#include "capture_base.h"
-#include "sensor_laser_2D.h"
+#include "base/capture/capture_base.h"
+#include "base/sensor/sensor_laser_2D.h"
 
 //laserscanutils includes
 #include "laser_scan_utils/laser_scan.h"
@@ -18,7 +18,6 @@ namespace wolf {
 
 WOLF_PTR_TYPEDEFS(CaptureLaser2D);
 
-
 class CaptureLaser2D : public CaptureBase
 {
     public:
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index a07ddc875..b867d2ec1 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -9,8 +9,8 @@
 #define SRC_CAPTURE_MOTION_H_
 
 // Wolf includes
-#include "capture_base.h"
-#include "motion_buffer.h"
+#include "base/capture/capture_base.h"
+#include "base/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 208f99fa4..eafb6ead7 100644
--- a/include/base/capture/capture_odom_2D.h
+++ b/include/base/capture/capture_odom_2D.h
@@ -8,9 +8,9 @@
 #ifndef CAPTURE_ODOM_2D_H_
 #define CAPTURE_ODOM_2D_H_
 
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
 
-#include "rotations.h"
+#include "base/rotations.h"
 
 namespace wolf
 {
@@ -44,8 +44,6 @@ inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const
     return delta;
 }
 
-
-
 } /* namespace wolf */
 
 #endif /* CAPTURE_ODOM_2D_H_ */
diff --git a/include/base/capture/capture_odom_3D.h b/include/base/capture/capture_odom_3D.h
index 21fee1943..d1f29508b 100644
--- a/include/base/capture/capture_odom_3D.h
+++ b/include/base/capture/capture_odom_3D.h
@@ -8,9 +8,9 @@
 #ifndef CAPTURE_ODOM_3D_H_
 #define CAPTURE_ODOM_3D_H_
 
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
 
-#include "rotations.h"
+#include "base/rotations.h"
 
 namespace wolf
 {
@@ -39,5 +39,4 @@ class CaptureOdom3D : public CaptureMotion
 
 } /* namespace wolf */
 
-
 #endif /* CAPTURE_ODOM_3D_H_ */
diff --git a/include/base/capture/capture_pose.h b/include/base/capture/capture_pose.h
index 9d47d39c9..51fb833e4 100644
--- a/include/base/capture/capture_pose.h
+++ b/include/base/capture/capture_pose.h
@@ -2,10 +2,10 @@
 #define CAPTURE_POSE_H_
 
 //Wolf includes
-#include "capture_base.h"
-#include "constraint_pose_2D.h"
-#include "constraint_pose_3D.h"
-#include "feature_pose.h"
+#include "base/capture/capture_base.h"
+#include "base/constraint/constraint_pose_2D.h"
+#include "base/constraint/constraint_pose_3D.h"
+#include "base/feature/feature_pose.h"
 
 //std includes
 //
diff --git a/include/base/capture/capture_velocity.h b/include/base/capture/capture_velocity.h
index 55b1bcf1a..4853fce2a 100644
--- a/include/base/capture/capture_velocity.h
+++ b/include/base/capture/capture_velocity.h
@@ -9,7 +9,7 @@
 #define _WOLF_CAPTURE_VELOCITY_H_
 
 //wolf includes
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
 
 namespace wolf {
 
diff --git a/include/base/capture/capture_void.h b/include/base/capture/capture_void.h
index 50b3b3c7d..b6b7a755d 100644
--- a/include/base/capture/capture_void.h
+++ b/include/base/capture/capture_void.h
@@ -2,8 +2,7 @@
 #define CAPTURE_VOID_H_
 
 //Wolf includes
-#include "capture_base.h"
-
+#include "base/capture/capture_base.h"
 
 namespace wolf {
 
diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h
index 02829cd1a..0bade4029 100644
--- a/include/base/capture/capture_wheel_joint_position.h
+++ b/include/base/capture/capture_wheel_joint_position.h
@@ -9,7 +9,7 @@
 #define CAPTURE_WHEEL_JOINT_POSITION_H_
 
 //wolf includes
-#include "../capture_motion.h"
+#include "base/capture/capture_motion.h"
 
 namespace wolf {
 
@@ -60,7 +60,6 @@ protected:
   Eigen::MatrixXs positions_cov_;
 };
 
-
 /// @todo Enforce some logic on the wheel joint pos data
 
 //template <typename E>
@@ -178,7 +177,6 @@ protected:
 
 //using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>;
 
-
 } // namespace wolf
 
 #endif /* CAPTURE_WHEEL_JOINT_POSITION_H_ */
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index 8852bc06d..5eb3436f5 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -7,10 +7,10 @@
 #include "glog/logging.h"
 
 //wolf includes
-#include "../solver/solver_manager.h"
-#include "cost_function_wrapper.h"
+#include "base/solver/solver_manager.h"
+#include "base/ceres_wrapper/cost_function_wrapper.h"
 #include "local_parametrization_wrapper.h"
-#include "create_numeric_diff_cost_function.h"
+#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
 
 namespace ceres {
 typedef std::shared_ptr<CostFunction>  CostFunctionPtr;
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
index ffd881914..119839dee 100644
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ b/include/base/ceres_wrapper/cost_function_wrapper.h
@@ -2,8 +2,8 @@
 #define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
 
 // WOLF
-#include "../wolf.h"
-#include "../constraint_analytic.h"
+#include "base/wolf.h"
+#include "base/constraint/constraint_analytic.h"
 
 // CERES
 #include "ceres/cost_function.h"
@@ -56,6 +56,4 @@ inline ConstraintBasePtr CostFunctionWrapper::getConstraintPtr() const
 
 } // namespace wolf
 
-
-
 #endif /* TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ */
diff --git a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
index d6650b203..8f8e9686b 100644
--- a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -12,9 +12,8 @@
 #include "ceres/numeric_diff_cost_function.h"
 
 // Constraints
-#include "../constraint_odom_2D.h"
-#include "../constraint_base.h"
-
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_base.h"
 
 namespace wolf {
 
@@ -29,7 +28,6 @@ std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSiz
                                                            T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_constraint_ptr).get());
 };
 
-
 inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr)
 {
 //    switch (_ctr_ptr->getTypeId())
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
index d9e0d9e82..4ae4182cc 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 "../wolf.h"
+#include "base/wolf.h"
 #include "ceres/ceres.h"
 
 namespace wolf {
@@ -38,7 +38,7 @@ using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapp
 
 } // namespace wolf
 
-#include "../local_parametrization_base.h"
+#include "base/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
index ae7fae7a1..d09a5147b 100644
--- a/include/base/ceres_wrapper/qr_manager.h
+++ b/include/base/ceres_wrapper/qr_manager.h
@@ -8,8 +8,8 @@
 #ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_
 #define SRC_CERES_WRAPPER_QR_MANAGER_H_
 
-#include "solver_manager.h"
-#include "sparse_utils.h"
+#include "base/solver/solver_manager.h"
+#include "base/solver_suitesparse/sparse_utils.h"
 
 namespace wolf
 {
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
index f7725df4a..6ffda3f7c 100644
--- a/include/base/ceres_wrapper/solver_manager.h
+++ b/include/base/ceres_wrapper/solver_manager.h
@@ -2,9 +2,9 @@
 #define SOLVER_MANAGER_H_
 
 //wolf includes
-#include "../wolf.h"
-#include "../state_block.h"
-#include "../constraint_base.h"
+#include "base/wolf.h"
+#include "base/state_block.h"
+#include "base/constraint/constraint_base.h"
 
 namespace wolf {
 
diff --git a/include/base/constraint/constraint_AHP.h b/include/base/constraint/constraint_AHP.h
index a97ac9840..340acc9c3 100644
--- a/include/base/constraint/constraint_AHP.h
+++ b/include/base/constraint/constraint_AHP.h
@@ -2,11 +2,11 @@
 #define CONSTRAINT_AHP_H
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "landmark_AHP.h"
-#include "sensor_camera.h"
-//#include "feature_point_image.h"
-#include "pinhole_tools.h"
+#include "base/constraint/constraint_autodiff.h"
+#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 <iomanip> //setprecision
 
@@ -68,7 +68,6 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr&   _ftr_ptr,
         ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>("AHP",
                                                             _landmark_ptr->getAnchorFrame(),
                                                             nullptr,
-                                                            nullptr,
                                                             _landmark_ptr,
                                                             _processor_ptr,
                                                             _apply_loss_function,
@@ -115,7 +114,6 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p,
 {
     using namespace Eigen;
 
-
     // All involved transforms typedef
     typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
 
@@ -198,5 +196,4 @@ inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr&   _ftr_ptr,
 
 } // namespace wolf
 
-
 #endif // CONSTRAINT_AHP_H
diff --git a/include/base/constraint/constraint_GPS_2D.h b/include/base/constraint/constraint_GPS_2D.h
index 5feb77fb9..2f85d0663 100644
--- a/include/base/constraint/constraint_GPS_2D.h
+++ b/include/base/constraint/constraint_GPS_2D.h
@@ -3,9 +3,9 @@
 #define CONSTRAINT_GPS_2D_H_
 
 //Wolf includes
-#include "wolf.h"
-#include "constraint_autodiff.h"
-#include "frame_base.h"
+#include "base/wolf.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
 
 namespace wolf {
     
diff --git a/include/base/constraint/constraint_GPS_pseudorange_2D.h b/include/base/constraint/constraint_GPS_pseudorange_2D.h
index 7a04463b9..3c15a0246 100644
--- a/include/base/constraint/constraint_GPS_pseudorange_2D.h
+++ b/include/base/constraint/constraint_GPS_pseudorange_2D.h
@@ -4,9 +4,9 @@
 #define LIGHT_SPEED_ 299792458
 
 //Wolf includes
-#include "sensor_GPS.h"
-#include "feature_GPS_pseudorange.h"
-#include "constraint_autodiff.h"
+#include "base/sensor/sensor_GPS.h"
+#include "base/feature/feature_GPS_pseudorange.h"
+#include "base/constraint/constraint_autodiff.h"
 
 //std
 #include <string>
@@ -31,10 +31,6 @@ class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudo
                                    bool _apply_loss_function = false, 
                                    ConstraintStatus _status = CTR_ACTIVE) :
            ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
-                                                                               nullptr,
-                                                                               nullptr,
-                                                                               nullptr,
-                                                                               nullptr,
                                                                                nullptr,
                                                                                _apply_loss_function,
                                                                                _status,
@@ -98,7 +94,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
     //Filling Eigen vectors
     Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect base frame
 
-
     /*
      * Base-to-map transform matrix
      */
@@ -175,7 +170,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
     T_lon_lat(2, 0) = T(sin(lat));
     T_lon_lat(2, 2) = T(cos(lat));
 
-
     Eigen::Matrix<T, 4, 4> T_lat_enu = Eigen::Matrix<T, 4, 4>::Zero();
     T_lat_enu(0, 2) = T_lat_enu(1, 0) = T_lat_enu(2, 1) = T_lat_enu(3, 3) = T(1);
 
@@ -190,7 +184,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
     //sensor position with respect to ecef coordinate system
     Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_ecef_map * sensor_p_map;
 
-
     /*
      * calculate the residual
      */
@@ -209,7 +202,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
     // normalizing by the covariance
     _residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0)));
 
-
     if (verbose_level_ >= 1)
     {
         aux.str(std::string());
diff --git a/include/base/constraint/constraint_GPS_pseudorange_3D.h b/include/base/constraint/constraint_GPS_pseudorange_3D.h
index 4c44d262e..02323d2c2 100644
--- a/include/base/constraint/constraint_GPS_pseudorange_3D.h
+++ b/include/base/constraint/constraint_GPS_pseudorange_3D.h
@@ -4,9 +4,9 @@
 #define LIGHT_SPEED 299792458
 
 //Wolf includes
-#include "sensor_GPS.h"
-#include "feature_GPS_pseudorange.h"
-#include "constraint_autodiff.h"
+#include "base/sensor/sensor_GPS.h"
+#include "base/feature/feature_GPS_pseudorange.h"
+#include "base/constraint/constraint_autodiff.h"
 
 namespace wolf {
 
@@ -45,7 +45,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
             //std::cout << "ConstraintGPSPseudorange3D()  pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
         }
 
-
         virtual ~ConstraintGPSPseudorange3D() = default;
 
         template<typename T>
@@ -66,7 +65,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
             return std::make_shared<ConstraintGPSPseudorange3D>(_feature_ptr);
         }
 
-
 };
 
 /*
@@ -115,7 +113,6 @@ inline bool ConstraintGPSPseudorange3D::operator ()(const T* const _vehicle_p, c
     for (int i = 0; i < 3; ++i)
         T_map_base(i, 3) = _vehicle_p[i];
 
-
     /*
      * Compute sensor_p wrt ECEF
      */
diff --git a/include/base/constraint/constraint_IMU.h b/include/base/constraint/constraint_IMU.h
index 9ccf54eeb..87dd49f0f 100644
--- a/include/base/constraint/constraint_IMU.h
+++ b/include/base/constraint/constraint_IMU.h
@@ -2,14 +2,13 @@
 #define CONSTRAINT_IMU_THETA_H_
 
 //Wolf includes
-#include "feature_IMU.h"
-#include "sensor_IMU.h"
-#include "constraint_autodiff.h"
-#include "rotations.h"
+#include "base/feature/feature_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/rotations.h"
 
 //Eigen include
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(ConstraintIMU);
@@ -101,7 +100,6 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3
         */
         Eigen::VectorXs expectation() const;
 
-
     private:
         /// Preintegrated delta
         Eigen::Vector3s dp_preint_;
@@ -183,8 +181,6 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr&    _ftr_ptr,
     //
 }
 
-
-
 template<typename T>
 inline bool ConstraintIMU::operator ()(const T* const _p1,
                                        const T* const _q1,
@@ -267,12 +263,10 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
      *   results in :
      *    res    = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
      *
-     *
      * NOTE: See optimization report at the end of this file for comparisons of both methods.
      */
 #define METHOD_1 // if commented, then METHOD_2 will be applied
 
-
     //needed typedefs
     typedef typename D1::Scalar T;
 
@@ -285,7 +279,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
 
     imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
 
-
     // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
 
     // 2.a. Compute the delta step in tangent space:   step = J_bias * (bias - bias_preint)
@@ -309,7 +302,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
               dp_step, do_step, dv_step,
               dp_correct, dq_correct, dv_correct);
 
-
     // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
     // Note the Dt here is zero because it's the delta-time between the same time stamps!
     Eigen::Matrix<T, 9, 1> d_error;
@@ -361,7 +353,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
     _res.segment(9,3)  = sqrt_A_r_dt_inv.cast<T>() * ab_error;
     _res.tail(3)       = sqrt_W_r_dt_inv.cast<T>() * wb_error;
 
-
     //////////////////////////////////////////////////////////////////////////////////////////////
     /////////////////////////////////       PRINT VALUES       ///////////////////////////////////
 #if 0
@@ -396,14 +387,10 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
     WOLF_TRACE("-----------------------------------------")
 #endif
     /////////////////////////////////////////////////////////////////////////////////////////////
-    /////////////////////////////////////////////////////////////////////////////////////////////
-
-
 
     return true;
 }
 
-
 inline Eigen::VectorXs ConstraintIMU::expectation() const
 {
     FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
@@ -448,25 +435,20 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> &        _p1,
     imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
 }
 
-
 } // namespace wolf
 
 #endif
 
-
 /*
  * Optimization results
  * ================================================
  *
- *
  * Using gtest_IMU.cpp
  *
  * Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
  *
  * You can verify this by looking at the 'Iterations' and 'Final costs' in the Ceres reports below.
  *
- *
- *
  * With Method 1:
  *
 [ RUN      ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
@@ -490,8 +472,6 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> &        _p1,
 [       OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
 [----------] 2 tests from Process_Constraint_IMU_ODO (120 ms total)
 *
-*
-*
 * With Method 2:
 *
 [ RUN      ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
@@ -515,5 +495,4 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> &        _p1,
 [       OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
 [----------] 2 tests from Process_Constraint_IMU_ODO (127 ms total)
 *
-*
 */
diff --git a/include/base/constraint/constraint_analytic.h b/include/base/constraint/constraint_analytic.h
index 17ff4a079..3b5015be0 100644
--- a/include/base/constraint/constraint_analytic.h
+++ b/include/base/constraint/constraint_analytic.h
@@ -3,7 +3,7 @@
 #define CONSTRAINT_ANALYTIC_H_
 
 //Wolf includes
-#include "constraint_base.h"
+#include "base/constraint/constraint_base.h"
 #include <Eigen/StdVector>
 
 namespace wolf {
@@ -37,7 +37,6 @@ class ConstraintAnalytic: public ConstraintBase
                            StateBlockPtr _state8Ptr = nullptr,
                            StateBlockPtr _state9Ptr = nullptr ) ;
 
-
         ConstraintAnalytic(const std::string&  _tp,
                            const FrameBasePtr& _frame_other_ptr,
                            const CaptureBasePtr& _capture_other_ptr,
@@ -57,7 +56,6 @@ class ConstraintAnalytic: public ConstraintBase
                            StateBlockPtr _state8Ptr = nullptr,
                            StateBlockPtr _state9Ptr = nullptr );
 
-
         virtual ~ConstraintAnalytic() = default;
 
         /** \brief Returns a vector of pointers to the states
diff --git a/include/base/constraint/constraint_autodiff.h b/include/base/constraint/constraint_autodiff.h
index ed61b0756..e6a687036 100644
--- a/include/base/constraint/constraint_autodiff.h
+++ b/include/base/constraint/constraint_autodiff.h
@@ -3,8 +3,8 @@
 #define CONSTRAINT_AUTODIFF_H_
 
 //Wolf includes
-#include "constraint_base.h"
-#include "state_block.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
 
 // CERES
 #include "ceres/jet.h"
@@ -299,7 +299,6 @@ class ConstraintAutodiff : public ConstraintBase
         }
 };
 
-
 ////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
 
 template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
@@ -2159,17 +2158,9 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
        }
 };
 
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //                                          STATIC CONST VECTORS INITIALIZATION
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // state_block_sizes_
 // 10 BLOCKS
 template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
diff --git a/include/base/constraint/constraint_autodiff_distance_3D.h b/include/base/constraint/constraint_autodiff_distance_3D.h
index a77e3880f..6e9452ff6 100644
--- a/include/base/constraint/constraint_autodiff_distance_3D.h
+++ b/include/base/constraint/constraint_autodiff_distance_3D.h
@@ -8,7 +8,7 @@
 #ifndef CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
 #define CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
 
-#include "constraint_autodiff.h"
+#include "base/constraint/constraint_autodiff.h"
 
 namespace wolf
 {
diff --git a/include/base/constraint/constraint_autodiff_trifocal.h b/include/base/constraint/constraint_autodiff_trifocal.h
index ac54629a6..d3844c350 100644
--- a/include/base/constraint/constraint_autodiff_trifocal.h
+++ b/include/base/constraint/constraint_autodiff_trifocal.h
@@ -2,9 +2,9 @@
 #define _CONSTRAINT_AUTODIFF_TRIFOCAL_H_
 
 //Wolf includes
-//#include "wolf.h"
-#include "constraint_autodiff.h"
-#include "sensor_camera.h"
+//#include "base/wolf.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/sensor/sensor_camera.h"
 
 #include <common_class/trifocaltensor.h>
 #include <vision_utils.h>
@@ -108,7 +108,6 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT
                                         MatrixBase<D3>& _J_e_m2,
                                         MatrixBase<D4>& _J_e_m3);
 
-
     private:
         FeatureBaseWPtr feature_prev_ptr_;  // To look for measurements
         SensorCameraPtr camera_ptr_;        // To look for intrinsics
@@ -118,10 +117,8 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT
 
 } // namespace wolf
 
-
-
 // Includes for implentation
-#include "rotations.h"
+#include "base/rotations.h"
 
 namespace wolf
 {
@@ -136,7 +133,6 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
         const ProcessorBasePtr& _processor_ptr,
         bool _apply_loss_function,
         ConstraintStatus _status) : ConstraintAutodiff( "TRIFOCAL PLP",
-                                                        nullptr,
                                                         nullptr,
                                                         _feature_origin_ptr,
                                                         nullptr,
@@ -217,7 +213,6 @@ inline FeatureBasePtr ConstraintAutodiffTrifocal::getFeaturePrevPtr()
     return feature_prev_ptr_.lock();
 }
 
-
 template<typename T>
 bool ConstraintAutodiffTrifocal::operator ()( const T* const _prev_pos,
                                               const T* const _prev_quat,
@@ -379,7 +374,6 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_
 
     J_e2_m3.setZero(); // Not involved in epipolar c1->c2
 
-
     // Compact Jacobians
     _J_e_m1.topRows(2) = J_e1_m1;
     _J_e_m1.row(2)     = J_e2_m1;
@@ -397,8 +391,6 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_
 
 }
 
-
 }    // namespace wolf
 
-
 #endif /* _CONSTRAINT_AUTODIFF_TRIFOCAL_H_ */
diff --git a/include/base/constraint/constraint_base.h b/include/base/constraint/constraint_base.h
index 3341a55c8..1a0b4f14f 100644
--- a/include/base/constraint/constraint_base.h
+++ b/include/base/constraint/constraint_base.h
@@ -7,8 +7,8 @@ class FeatureBase;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
 
 //std includes
 
@@ -178,16 +178,15 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
 //        void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar
 };
 
-
 }
 
 // IMPLEMENTATION //
 
-#include "problem.h"
-#include "frame_base.h"
-#include "feature_base.h"
-#include "sensor_base.h"
-#include "landmark_base.h"
+#include "base/problem.h"
+#include "base/frame_base.h"
+#include "base/feature/feature_base.h"
+#include "base/sensor/sensor_base.h"
+#include "base/landmark/landmark_base.h"
 
 namespace wolf{
 
diff --git a/include/base/constraint/constraint_block_absolute.h b/include/base/constraint/constraint_block_absolute.h
index fe8eec7a9..22b4ee1b1 100644
--- a/include/base/constraint/constraint_block_absolute.h
+++ b/include/base/constraint/constraint_block_absolute.h
@@ -9,9 +9,8 @@
 #define CONSTRAINT_BLOCK_ABSOLUTE_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
 
 namespace wolf {
 
diff --git a/include/base/constraint/constraint_container.h b/include/base/constraint/constraint_container.h
index c796d1e32..1e578f10d 100644
--- a/include/base/constraint/constraint_container.h
+++ b/include/base/constraint/constraint_container.h
@@ -2,9 +2,9 @@
 #define CONSTRAINT_CONTAINER_H_
 
 //Wolf includes
-#include "wolf.h"
-#include "constraint_autodiff.h"
-#include "landmark_container.h"
+#include "base/wolf.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/landmark/landmark_container.h"
 
 namespace wolf {
     
@@ -116,7 +116,6 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
 			return true;
 		}
 
-
   public:
     static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
                                     const NodeBasePtr& _correspondant_ptr,
diff --git a/include/base/constraint/constraint_corner_2D.h b/include/base/constraint/constraint_corner_2D.h
index 96afcfd50..4692ad29a 100644
--- a/include/base/constraint/constraint_corner_2D.h
+++ b/include/base/constraint/constraint_corner_2D.h
@@ -2,8 +2,8 @@
 #define CONSTRAINT_CORNER_2D_THETA_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "landmark_corner_2D.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/landmark/landmark_corner_2D.h"
 
 namespace wolf {
 
@@ -111,6 +111,4 @@ inline bool ConstraintCorner2D::operator ()(const T* const _robotP, const T* con
 
 } // namespace wolf
 
-
-
 #endif
diff --git a/include/base/constraint/constraint_diff_drive.h b/include/base/constraint/constraint_diff_drive.h
index 8d6a898a0..017223784 100644
--- a/include/base/constraint/constraint_diff_drive.h
+++ b/include/base/constraint/constraint_diff_drive.h
@@ -9,10 +9,10 @@
 #define WOLF_CONSTRAINT_DIFF_DRIVE_H_
 
 //Wolf includes
-#include "../constraint_autodiff.h"
-#include "../frame_base.h"
-#include "../features/feature_diff_drive.h"
-#include "../captures/capture_wheel_joint_position.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/feature/feature_diff_drive.h"
+#include "base/capture/capture_wheel_joint_position.h"
 
 namespace
 {
diff --git a/include/base/constraint/constraint_epipolar.h b/include/base/constraint/constraint_epipolar.h
index 3172173cf..2887af376 100644
--- a/include/base/constraint/constraint_epipolar.h
+++ b/include/base/constraint/constraint_epipolar.h
@@ -1,7 +1,7 @@
 #ifndef CONSTRAINT_EPIPOLAR_H
 #define CONSTRAINT_EPIPOLAR_H
 
-#include "constraint_base.h"
+#include "base/constraint/constraint_base.h"
 
 namespace wolf {
 
@@ -19,7 +19,6 @@ class ConstraintEpipolar : public ConstraintBase
 
         virtual ~ConstraintEpipolar() = default;
 
-
         /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
         **/
         virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};
diff --git a/include/base/constraint/constraint_fix_bias.h b/include/base/constraint/constraint_fix_bias.h
index ed69c9049..55dbbf8db 100644
--- a/include/base/constraint/constraint_fix_bias.h
+++ b/include/base/constraint/constraint_fix_bias.h
@@ -3,15 +3,14 @@
 #define CONSTRAINT_FIX_BIAS_H_
 
 //Wolf includes
-#include "capture_IMU.h"
-#include "feature_IMU.h"
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-#include "rotations.h"
+#include "base/capture/capture_IMU.h"
+#include "base/feature/feature_IMU.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
 
 //#include "ceres/jet.h"
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(ConstraintFixBias);
diff --git a/include/base/constraint/constraint_odom_2D.h b/include/base/constraint/constraint_odom_2D.h
index 11333302f..5df37fbb9 100644
--- a/include/base/constraint/constraint_odom_2D.h
+++ b/include/base/constraint/constraint_odom_2D.h
@@ -2,8 +2,8 @@
 #define CONSTRAINT_ODOM_2D_THETA_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
 
 //#include "ceres/jet.h"
 
@@ -73,7 +73,6 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1
     // Residuals
     res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
 
-
     ////////////////////////////////////////////////////////
     // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
 //    using ceres::Jet;
@@ -92,7 +91,6 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1
 //    }
     ////////////////////////////////////////////////////////
 
-
     return true;
 }
 
diff --git a/include/base/constraint/constraint_odom_2D_analytic.h b/include/base/constraint/constraint_odom_2D_analytic.h
index d69655e27..43048d3ff 100644
--- a/include/base/constraint/constraint_odom_2D_analytic.h
+++ b/include/base/constraint/constraint_odom_2D_analytic.h
@@ -2,7 +2,7 @@
 #define CONSTRAINT_ODOM_2D_ANALYTIC_H_
 
 //Wolf includes
-#include "constraint_relative_2D_analytic.h"
+#include "base/constraint/constraint_relative_2D_analytic.h"
 #include <Eigen/StdVector>
 
 namespace wolf {
@@ -93,7 +93,6 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
 //            jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
 //        }
 
-
     public:
         static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
                                               const NodeBasePtr& _correspondant_ptr,
diff --git a/include/base/constraint/constraint_odom_3D.h b/include/base/constraint/constraint_odom_3D.h
index 26811e470..9047b7891 100644
--- a/include/base/constraint/constraint_odom_3D.h
+++ b/include/base/constraint/constraint_odom_3D.h
@@ -8,8 +8,8 @@
 #ifndef CONSTRAINT_ODOM_3D_H_
 #define CONSTRAINT_ODOM_3D_H_
 
-#include "constraint_autodiff.h"
-#include "rotations.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/rotations.h"
 
 namespace wolf
 {
@@ -49,7 +49,6 @@ class ConstraintOdom3D : public ConstraintAutodiff<ConstraintOdom3D,6,3,4,3,4>
         template<typename T>
         void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
 
-
 };
 
 template<typename T>
@@ -71,7 +70,6 @@ inline void ConstraintOdom3D::printRes (const  Eigen::Matrix<Scalar,6,1> & r) co
     std::cout << r.transpose() << std::endl;
 }
 
-
 inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr,
                                           const FrameBasePtr& _frame_past_ptr,
                                           const ProcessorBasePtr& _processor_ptr,
diff --git a/include/base/constraint/constraint_point_2D.h b/include/base/constraint/constraint_point_2D.h
index 50d190409..1900991f7 100644
--- a/include/base/constraint/constraint_point_2D.h
+++ b/include/base/constraint/constraint_point_2D.h
@@ -2,9 +2,9 @@
 #define CONSTRAINT_POINT_2D_THETA_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
 
 namespace wolf {
     
diff --git a/include/base/constraint/constraint_point_to_line_2D.h b/include/base/constraint/constraint_point_to_line_2D.h
index 324ae24d6..842b84a80 100644
--- a/include/base/constraint/constraint_point_to_line_2D.h
+++ b/include/base/constraint/constraint_point_to_line_2D.h
@@ -2,9 +2,9 @@
 #define CONSTRAINT_POINT_TO_LINE_2D_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
 
 namespace wolf {
     
diff --git a/include/base/constraint/constraint_pose_2D.h b/include/base/constraint/constraint_pose_2D.h
index 652c1c377..b6fe79ef3 100644
--- a/include/base/constraint/constraint_pose_2D.h
+++ b/include/base/constraint/constraint_pose_2D.h
@@ -3,13 +3,12 @@
 #define CONSTRAINT_POSE_2D_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-#include "rotations.h"
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
 
 //#include "ceres/jet.h"
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(ConstraintPose2D);
diff --git a/include/base/constraint/constraint_pose_3D.h b/include/base/constraint/constraint_pose_3D.h
index 8823eefb7..5fa1ec6c2 100644
--- a/include/base/constraint/constraint_pose_3D.h
+++ b/include/base/constraint/constraint_pose_3D.h
@@ -3,10 +3,9 @@
 #define CONSTRAINT_POSE_3D_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "frame_base.h"
-#include "rotations.h"
-
+#include "base/constraint/constraint_autodiff.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
 
 namespace wolf {
 
diff --git a/include/base/constraint/constraint_quaternion_absolute.h b/include/base/constraint/constraint_quaternion_absolute.h
index 1b147a03d..eb2e2431a 100644
--- a/include/base/constraint/constraint_quaternion_absolute.h
+++ b/include/base/constraint/constraint_quaternion_absolute.h
@@ -9,11 +9,10 @@
 #define CONSTRAINT_QUATERNION_ABSOLUTE_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
-#include "local_parametrization_quaternion.h"
-#include "frame_base.h"
-#include "rotations.h"
-
+#include "base/constraint/constraint_autodiff.h"
+#include "base/local_parametrization_quaternion.h"
+#include "base/frame_base.h"
+#include "base/rotations.h"
 
 namespace wolf {
 
diff --git a/include/base/constraint/constraint_relative_2D_analytic.h b/include/base/constraint/constraint_relative_2D_analytic.h
index 32b8b3059..a9655a914 100644
--- a/include/base/constraint/constraint_relative_2D_analytic.h
+++ b/include/base/constraint/constraint_relative_2D_analytic.h
@@ -2,8 +2,8 @@
 #define CONSTRAINT_RELATIVE_2D_ANALYTIC_H_
 
 //Wolf includes
-#include "constraint_analytic.h"
-#include "landmark_base.h"
+#include "base/constraint/constraint_analytic.h"
+#include "base/landmark/landmark_base.h"
 #include <Eigen/StdVector>
 
 namespace wolf {
@@ -93,7 +93,6 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
 
 };
 
-
 /// IMPLEMENTATION ///
 
 inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals(
diff --git a/src/sensor/diff_drive_tools.h b/include/base/diff_drive_tools.h
similarity index 99%
rename from src/sensor/diff_drive_tools.h
rename to include/base/diff_drive_tools.h
index ccf243cc1..142b8c502 100644
--- a/src/sensor/diff_drive_tools.h
+++ b/include/base/diff_drive_tools.h
@@ -8,7 +8,7 @@
 #ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
 #define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
 
-#include "../eigen_assert.h"
+#include "base/eigen_assert.h"
 
 namespace wolf {
 
@@ -359,7 +359,6 @@ void integrateExact(const Eigen::MatrixBase<T0> &data,
   delta_cov = jacobian * data_cov * jacobian.transpose();
 }
 
-
 /**
  * @brief integrate. Helper function to call either
  * `integrateRung` or `integrateExact` depending on the
@@ -420,6 +419,6 @@ Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov(
 
 } // namespace wolf
 
-#include "diff_drive_tools.hpp"
+#include "base/diff_drive_tools.hpp"
 
 #endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/src/sensor/diff_drive_tools.hpp b/include/base/diff_drive_tools.hpp
similarity index 100%
rename from src/sensor/diff_drive_tools.hpp
rename to include/base/diff_drive_tools.hpp
diff --git a/include/base/eigen_predicates.h b/include/base/eigen_predicates.h
index 65e737b1b..55f2eeeb7 100644
--- a/include/base/eigen_predicates.h
+++ b/include/base/eigen_predicates.h
@@ -8,7 +8,7 @@
 #ifndef _WOLF_EIGEN_PREDICATES_H_
 #define _WOLF_EIGEN_PREDICATES_H_
 
-#include "rotations.h"
+#include "base/rotations.h"
 
 namespace wolf {
 
@@ -97,5 +97,4 @@ auto pred_angle_zero = [](const Scalar lhs, const Scalar precision)
 
 } // namespace wolf
 
-
 #endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/include/base/factory.h b/include/base/factory.h
index c63d6df8d..b161126e4 100644
--- a/include/base/factory.h
+++ b/include/base/factory.h
@@ -9,7 +9,7 @@
 #define FACTORY_H_
 
 // wolf
-#include "wolf.h"
+#include "base/wolf.h"
 
 // std
 #include <string>
@@ -308,8 +308,6 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass()
 
 } // namespace wolf
 
-
-
 namespace wolf
 {
 
@@ -349,7 +347,7 @@ inline std::string LandmarkFactory::getClass()
 // Frames
 class TimeStamp;
 } // namespace wolf
-#include "frame_base.h"
+#include "base/frame_base.h"
 namespace wolf{
 typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
 template<>
@@ -378,7 +376,6 @@ inline std::string FrameFactory::getClass()
   namespace{ const bool WOLF_UNUSED FrameName##Registered = \
     wolf::FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
 
-
 } /* namespace wolf */
 
 #endif /* FACTORY_H_ */
diff --git a/include/base/feature/feature_GPS_fix.h b/include/base/feature/feature_GPS_fix.h
index 436d7f593..688176fdf 100644
--- a/include/base/feature/feature_GPS_fix.h
+++ b/include/base/feature/feature_GPS_fix.h
@@ -2,8 +2,8 @@
 #define FEATURE_GPS_FIX_H_
 
 //Wolf includes
-#include "constraint_GPS_2D.h"
-#include "feature_base.h"
+#include "base/constraint/constraint_GPS_2D.h"
+#include "base/feature/feature_base.h"
 
 //std includes
 
diff --git a/include/base/feature/feature_GPS_pseudorange.h b/include/base/feature/feature_GPS_pseudorange.h
index 5adaa5b6c..57f327e25 100644
--- a/include/base/feature/feature_GPS_pseudorange.h
+++ b/include/base/feature/feature_GPS_pseudorange.h
@@ -3,12 +3,11 @@
 #define FEATURE_GPS_PSEUDORANGE_H_
 
 //Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 //std includes
 #include <iomanip>
 
-
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(FeatureGPSPseudorange);
diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h
index 37e6884db..dfce11ff7 100644
--- a/include/base/feature/feature_IMU.h
+++ b/include/base/feature/feature_IMU.h
@@ -2,13 +2,12 @@
 #define FEATURE_IMU_H_
 
 //Wolf includes
-#include "capture_IMU.h"
-#include "feature_base.h"
-#include "wolf.h"
+#include "base/capture/capture_IMU.h"
+#include "base/feature/feature_base.h"
+#include "base/wolf.h"
 
 //std includes
 
-
 namespace wolf {
 
 //WOLF_PTR_TYPEDEFS(CaptureIMU);
@@ -18,7 +17,6 @@ class FeatureIMU : public FeatureBase
 {
     public:
 
-
         /** \brief Constructor from and measures
          *
          * \param _measurement the measurement
@@ -58,7 +56,6 @@ class FeatureIMU : public FeatureBase
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };
 
-
 inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const
 {
     return acc_bias_preint_;
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index 1eb4fbf85..1ab46cccc 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -8,14 +8,13 @@ class ConstraintBase;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
 
 //std includes
 
 namespace wolf {
 
-
 //class FeatureBase
 class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
 {
@@ -97,7 +96,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
 
 // IMPLEMENTATION
 
-#include "constraint_base.h"
+#include "base/constraint/constraint_base.h"
 
 namespace wolf{
 
diff --git a/include/base/feature/feature_corner_2D.h b/include/base/feature/feature_corner_2D.h
index d43b82025..8e206749d 100644
--- a/include/base/feature/feature_corner_2D.h
+++ b/include/base/feature/feature_corner_2D.h
@@ -2,11 +2,10 @@
 #define FEATURE_CORNER_2D_H_
 
 //Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 //std includes
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(FeatureCorner2D);
diff --git a/src/feature/feature_diff_drive.h b/include/base/feature/feature_diff_drive.h
similarity index 95%
rename from src/feature/feature_diff_drive.h
rename to include/base/feature/feature_diff_drive.h
index 5c837469b..52877f4f5 100644
--- a/src/feature/feature_diff_drive.h
+++ b/include/base/feature/feature_diff_drive.h
@@ -9,7 +9,7 @@
 #define _WOLF_FEATURE_DIFF_DRIVE_H_
 
 //Wolf includes
-#include "../feature_base.h"
+#include "base/feature/feature_base.h"
 
 namespace wolf {
 
diff --git a/include/base/feature/feature_line_2D.h b/include/base/feature/feature_line_2D.h
index cd99621a0..5d5e6162d 100644
--- a/include/base/feature/feature_line_2D.h
+++ b/include/base/feature/feature_line_2D.h
@@ -2,13 +2,12 @@
 #define FEATURE_LINE_2D_H_
 
 //Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(FeatureLine2D);
     
-    
 /** \brief class FeatureLine2D
  * 
  * Line segment feature. 
diff --git a/include/base/feature/feature_match.h b/include/base/feature/feature_match.h
index e776b1fcd..b96ce0023 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 "wolf.h"
+#include "base/wolf.h"
 
 //wolf nampseace
 namespace wolf {
@@ -34,4 +34,3 @@ struct FeatureMatch
 
 #endif
 
-
diff --git a/include/base/feature/feature_motion.h b/include/base/feature/feature_motion.h
index 61b9bb59a..92331b6b1 100644
--- a/include/base/feature/feature_motion.h
+++ b/include/base/feature/feature_motion.h
@@ -8,8 +8,8 @@
 #ifndef FEATURE_MOTION_H_
 #define FEATURE_MOTION_H_
 
-#include "feature_base.h"
-#include "capture_motion.h"
+#include "base/feature/feature_base.h"
+#include "base/capture/capture_motion.h"
 
 namespace wolf
 {
diff --git a/include/base/feature/feature_odom_2D.h b/include/base/feature/feature_odom_2D.h
index 3a43ee64f..92fd84fa7 100644
--- a/include/base/feature/feature_odom_2D.h
+++ b/include/base/feature/feature_odom_2D.h
@@ -2,13 +2,12 @@
 #define FEATURE_ODOM_2D_H_
 
 //Wolf includes
-#include "feature_base.h"
-#include "constraint_odom_2D.h"
-#include "constraint_odom_2D_analytic.h"
+#include "base/feature/feature_base.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_odom_2D_analytic.h"
 
 //std includes
 
-
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(FeatureOdom2D);
diff --git a/include/base/feature/feature_point_image.h b/include/base/feature/feature_point_image.h
index 6e82b4305..aa07d80df 100644
--- a/include/base/feature/feature_point_image.h
+++ b/include/base/feature/feature_point_image.h
@@ -1,9 +1,8 @@
 #ifndef FEATURE_IMAGE_H
 #define FEATURE_IMAGE_H
 
-
 //Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 // Vision Utils includes
 #include <vision_utils.h>
diff --git a/include/base/feature/feature_polyline_2D.h b/include/base/feature/feature_polyline_2D.h
index 5c621a0bf..d289a58f3 100644
--- a/include/base/feature/feature_polyline_2D.h
+++ b/include/base/feature/feature_polyline_2D.h
@@ -8,7 +8,7 @@
 #ifndef FEATURE_POLYLINE_2D_H_
 #define FEATURE_POLYLINE_2D_H_
 
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 namespace wolf
 {
diff --git a/include/base/feature/feature_pose.h b/include/base/feature/feature_pose.h
index f4d7ba061..1431f9207 100644
--- a/include/base/feature/feature_pose.h
+++ b/include/base/feature/feature_pose.h
@@ -1,9 +1,8 @@
 #ifndef FEATURE_POSE_H_
 #define FEATURE_POSE_H_
 
-
 //Wolf includes
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 //std includes
 //
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index f7e5f8379..16d13dbee 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -10,9 +10,9 @@ class StateBlock;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "time_stamp.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/time_stamp.h"
+#include "base/node_base.h"
 
 //std includes
 
@@ -26,7 +26,6 @@ typedef enum
     KEY_FRAME = 1       ///< key frame. It plays at optimizations.
 } FrameType;
 
-
 //class FrameBase
 class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
 {
@@ -68,9 +67,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         virtual ~FrameBase();
         virtual void remove();
 
-
-
-
         // Frame properties -----------------------------------------------
     public:
         unsigned int id();
@@ -160,14 +156,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
 
 // IMPLEMENTATION //
 
-#include "trajectory_base.h"
-#include "capture_base.h"
-#include "constraint_base.h"
-#include "state_block.h"
+#include "base/trajectory_base.h"
+#include "base/capture/capture_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
 
 namespace wolf {
 
-
 inline unsigned int FrameBase::id()
 {
     return frame_id_;
diff --git a/include/base/hardware_base.h b/include/base/hardware_base.h
index 9d60cdd10..f3679dc4a 100644
--- a/include/base/hardware_base.h
+++ b/include/base/hardware_base.h
@@ -8,8 +8,8 @@ class SensorBase;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
 
 namespace wolf {
 
diff --git a/include/base/landmark/landmark_AHP.h b/include/base/landmark/landmark_AHP.h
index e8d695b19..a706f87da 100644
--- a/include/base/landmark/landmark_AHP.h
+++ b/include/base/landmark/landmark_AHP.h
@@ -2,7 +2,7 @@
 #define LANDMARK_AHP_H
 
 //Wolf includes
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
 
 // yaml
 #include <yaml-cpp/yaml.h>
@@ -10,7 +10,6 @@
 // Vision utils
 #include <vision_utils/vision_utils.h>
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(LandmarkAHP);
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 66485387a..8b365e9be 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -8,9 +8,9 @@ class StateBlock;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
 
 //std includes
 
@@ -19,7 +19,6 @@ class StateBlock;
 
 namespace wolf {
 
-
 //class LandmarkBase
 class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
 {
@@ -75,7 +74,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         bool getCovariance(Eigen::MatrixXs& _cov) const;
         Eigen::MatrixXs getCovariance() const;
 
-
     protected:
         virtual void removeStateBlocks();
 
@@ -85,7 +83,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         Scalar getDescriptor(unsigned int _ii) const;
         void setDescriptor(const Eigen::VectorXs& _descriptor);
 
-
         // Navigate wolf tree
         virtual void setProblem(ProblemPtr _problem) final;
 
@@ -100,9 +97,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
 }
 
-#include "map_base.h"
-#include "constraint_base.h"
-#include "state_block.h"
+#include "base/map_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
 
 namespace wolf{
 
diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h
index 0f51f3c5a..65a2c5b71 100644
--- a/include/base/landmark/landmark_container.h
+++ b/include/base/landmark/landmark_container.h
@@ -3,8 +3,8 @@
 #define LANDMARK_CONTAINER_H_
 
 //Wolf includes
-#include "landmark_base.h"
-#include "wolf.h"
+#include "base/landmark/landmark_base.h"
+#include "base/wolf.h"
 
 // Std includes
 
@@ -36,7 +36,6 @@ class LandmarkContainer : public LandmarkBase
 		 *
 		 *          B ---------------------------- A
          *          |                              |
-         *          |                              |
          *          |           <---+              |
          *          |               |              |
          *          |               v              |
@@ -103,7 +102,6 @@ class LandmarkContainer : public LandmarkBase
         
 };
 
-
 } // namespace wolf
 
 #endif
diff --git a/include/base/landmark/landmark_corner_2D.h b/include/base/landmark/landmark_corner_2D.h
index 1eb99a975..c1ce86fbd 100644
--- a/include/base/landmark/landmark_corner_2D.h
+++ b/include/base/landmark/landmark_corner_2D.h
@@ -3,11 +3,10 @@
 #define LANDMARK_CORNER_H_
 
 //Wolf includes
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
 
 // Std includes
 
-
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(LandmarkCorner2D);
diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h
index 514cd550b..3a0f84c34 100644
--- a/include/base/landmark/landmark_line_2D.h
+++ b/include/base/landmark/landmark_line_2D.h
@@ -3,17 +3,15 @@
 #define LANDMARK_LINE_2D_H_
 
 //Wolf includes
-#include "landmark_base.h"
-#include "wolf.h"
+#include "base/landmark/landmark_base.h"
+#include "base/wolf.h"
 
 //std includes
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(LandmarkLine2D);
 
-
 //class LandmarkLine2D
 class LandmarkLine2D : public LandmarkBase
 {
diff --git a/include/base/landmark/landmark_match.h b/include/base/landmark/landmark_match.h
index ae47aa619..0c5d3a491 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 "wolf.h"
+#include "base/wolf.h"
 
 //wolf nampseace
 namespace wolf {
@@ -33,7 +33,6 @@ struct LandmarkMatch
     }
 };
 
-
 }//end namespace
 
 #endif
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index a44a00b4c..4414a69f6 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -9,7 +9,7 @@
 #define LANDMARK_POLYLINE_2D_H_
 
 // Wolf
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
 
 // STL
 #include <deque>
diff --git a/include/base/local_parametrization_angle.h b/include/base/local_parametrization_angle.h
index 2445de112..912237c07 100644
--- a/include/base/local_parametrization_angle.h
+++ b/include/base/local_parametrization_angle.h
@@ -8,8 +8,8 @@
 #ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
 #define LOCAL_PARAMETRIZATION_ANGLE_H_
 
-#include "local_parametrization_base.h"
-#include "rotations.h"
+#include "base/local_parametrization_base.h"
+#include "base/rotations.h"
 
 namespace wolf
 {
diff --git a/include/base/local_parametrization_base.h b/include/base/local_parametrization_base.h
index acb9ec683..3b7dcc38d 100644
--- a/include/base/local_parametrization_base.h
+++ b/include/base/local_parametrization_base.h
@@ -8,8 +8,7 @@
 #ifndef LOCAL_PARAMETRIZATION_BASE_H_
 #define LOCAL_PARAMETRIZATION_BASE_H_
 
-#include "wolf.h"
-
+#include "base/wolf.h"
 
 namespace wolf {
 
diff --git a/include/base/local_parametrization_homogeneous.h b/include/base/local_parametrization_homogeneous.h
index 852c9aba7..f06dd2fb1 100644
--- a/include/base/local_parametrization_homogeneous.h
+++ b/include/base/local_parametrization_homogeneous.h
@@ -8,8 +8,7 @@
 #ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
 #define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
 
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/local_parametrization_polyline_extreme.h b/include/base/local_parametrization_polyline_extreme.h
index 62f7760d7..3a75a8a0f 100644
--- a/include/base/local_parametrization_polyline_extreme.h
+++ b/include/base/local_parametrization_polyline_extreme.h
@@ -8,8 +8,7 @@
 #ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
 #define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_
 
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/local_parametrization_quaternion.h b/include/base/local_parametrization_quaternion.h
index 6cbcae7bb..f92676558 100644
--- a/include/base/local_parametrization_quaternion.h
+++ b/include/base/local_parametrization_quaternion.h
@@ -8,8 +8,7 @@
 #ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
 #define LOCAL_PARAMETRIZATION_QUATERNION_H_
 
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/include/base/logging.h b/include/base/logging.h
index 03d21c002..3593fc718 100644
--- a/include/base/logging.h
+++ b/include/base/logging.h
@@ -17,7 +17,7 @@
 #include "spdlog/fmt/bundled/ostream.h"
 
 // Wolf includes
-#include "singleton.h"
+#include "base/singleton.h"
 
 namespace wolf {
 namespace internal {
diff --git a/include/base/map_base.h b/include/base/map_base.h
index ef08f9469..f9e5d32b6 100644
--- a/include/base/map_base.h
+++ b/include/base/map_base.h
@@ -9,8 +9,8 @@ class LandmarkBase;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
 
 //std includes
 
diff --git a/include/base/motion_buffer.h b/include/base/motion_buffer.h
index 33dfa226e..f8d0ad5be 100644
--- a/include/base/motion_buffer.h
+++ b/include/base/motion_buffer.h
@@ -8,8 +8,8 @@
 #ifndef SRC_MOTIONBUFFER_H_
 #define SRC_MOTIONBUFFER_H_
 
-#include "wolf.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/time_stamp.h"
 
 #include <list>
 #include <algorithm>
@@ -51,7 +51,6 @@ struct Motion
 
 }; ///< One instance of the buffered data, corresponding to a particular time stamp.
 
-
 /** \brief class for motion buffers.
  *
  * It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled
@@ -117,7 +116,6 @@ inline void MotionBuffer::setCalibrationPreint(const VectorXs& _calib_preint)
     calib_preint_ = _calib_preint;
 }
 
-
 } // namespace wolf
 
 #endif /* SRC_MOTIONBUFFER_H_ */
diff --git a/include/base/node_base.h b/include/base/node_base.h
index bedc86612..2426f53a5 100644
--- a/include/base/node_base.h
+++ b/include/base/node_base.h
@@ -2,8 +2,7 @@
 #define NODE_BASE_H_
 
 // Wolf includes
-#include "wolf.h"
-
+#include "base/wolf.h"
 
 namespace wolf {
 
diff --git a/include/base/pinhole_tools.h b/include/base/pinhole_tools.h
index ac721bd97..46cb1fb9d 100644
--- a/include/base/pinhole_tools.h
+++ b/include/base/pinhole_tools.h
@@ -7,12 +7,11 @@
  * \date 06/04/2010
  * \author jsola
  *
- *
  *  ## Add a description here ##
  *
  */
 
-#include "wolf.h"
+#include "base/wolf.h"
 
 #include <iostream>
 
@@ -131,7 +130,6 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
     _dist = _v.norm();
 }
 
-
 /**
  * Canonical back-projection.
  * \param _u the 2D point in the image plane
@@ -262,7 +260,6 @@ Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1>
     }
 }
 
-
 /**
  * Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u, with jacobians
  * \param d the radial distortion parameters vector
@@ -398,7 +395,6 @@ void undistortPoint(const MatrixBase<Derived1>& c,
     }
 }
 
-
 /**
  * Pixellization from k = [u_0, v_0, a_u, a_v]
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -426,8 +422,6 @@ Matrix<typename Derived2::Scalar, 2, 1> pixellizePoint(const MatrixBase<Derived1
     return u;
 }
 
-
-
 /**
  * Pixellization from k = [u_0, v_0, a_u, a_v] with jacobians
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -459,7 +453,6 @@ void pixellizePoint(const MatrixBase<Derived1>& k,
     U_ud(1, 1) = a_v;
 }
 
-
 /**
  * Depixellization from k = [u_0, v_0, a_u, a_v]
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -487,7 +480,6 @@ Matrix<typename Derived2::Scalar, 2, 1> depixellizePoint(const MatrixBase<Derive
     return ud;
 }
 
-
 /**
  * Depixellization from k = [u_0, v_0, a_u, a_v], with Jacobians
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -518,7 +510,6 @@ void depixellizePoint(const MatrixBase<Derived1>&   k,
     UD_u(1, 1) = 1.0 / a_v;
 }
 
-
 /**
  * Project a point into a pin-hole camera with radial distortion
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -594,8 +585,6 @@ void projectPoint(const MatrixBase<Derived1>& k,
     u = pixellizePoint( k, distortPoint( d, up ));
 }
 
-
-
 /**
  * Project a point into a pin-hole camera with radial distortion
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -630,7 +619,6 @@ void projectPoint(const MatrixBase<Derived1>& k,
     U_v.noalias() = U_ud * UD_up * UP_v;
 }
 
-
 /**
  * Back-Project a point from a pin-hole camera with radial distortion
  * \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
@@ -688,7 +676,6 @@ void backprojectPoint(const MatrixBase<Derived1>&       k,
     P_u.noalias() = P_up * UP_ud * UD_u;
 }
 
-
 /**
  * Determine if a pixel is inside the region of interest
  * \param pix the pixel to test
@@ -713,7 +700,6 @@ bool isInImage(const MatrixBase<VPix> & pix, const int & width, const int & heig
     return isInRoi(pix, 0, 0, width, height);
 }
 
-
 /**
  * Compute distortion correction parameters.
  *
@@ -764,7 +750,6 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
         // this does not work:
         // jmath::LinearSolvers::solve_Cholesky(Rd, (rc - rd), c);
 
-
         // therefore we solve manually the pseudo-inverse:
         Eigen::MatrixXs RdtRd(size, size);
         RdtRd = Rd.transpose() * Rd;
@@ -780,9 +765,6 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
 
 } // namespace pinhole
 
-
-
 } // namespace wolf
 
-
 #endif // PINHOLETOOLS_H
diff --git a/include/base/problem.h b/include/base/problem.h
index 70cb8b7c4..f0ade3c32 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -14,16 +14,14 @@ struct ProcessorParamsBase;
 }
 
 //wolf includes
-#include "wolf.h"
-#include "frame_base.h"
-#include "state_block.h"
+#include "base/wolf.h"
+#include "base/frame_base.h"
+#include "base/state_block.h"
 
 // std includes
 
-
 namespace wolf {
 
-
 enum Notification
 {
     ADD,
@@ -91,7 +89,6 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         SensorBasePtr getSensorPtr(const std::string& _sensor_name);
 
-
         /** \brief Factory method to install (create, and add to sensor) processors only from its properties
          *
          * This method creates a Processor, and adds it to the specified sensor's list of processors
@@ -129,7 +126,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         void clearProcessorMotion();
         ProcessorMotionPtr& getProcessorMotionPtr();
 
-
         // Trajectory branch ----------------------------------
         TrajectoryBasePtr getTrajectoryPtr();
         virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
@@ -198,7 +194,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         FrameBasePtr    getLastKeyFramePtr      ( );
         FrameBasePtr    closestKeyFrameToTimeStamp(const TimeStamp& _ts);
 
-
         /** \brief Give the permission to a processor to create a new keyFrame
          *
          * This should implement a black list of processors that have forbidden keyframe creation.
@@ -225,8 +220,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         Eigen::VectorXs zeroState ( );
         bool priorIsSet() const;
 
-
-
         // Map branch -----------------------------------------
         MapBasePtr getMapPtr();
         LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
@@ -235,8 +228,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         void saveMap(const std::string& _filename_dot_yaml, //
                      const std::string& _map_name = "Map automatically saved by Wolf");
 
-
-
         // Covariances -------------------------------------------
         void clearCovariance();
         void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov);
@@ -329,5 +320,4 @@ inline std::map<ConstraintBasePtr,Notification>& Problem::getConstraintNotificat
 
 } // namespace wolf
 
-
 #endif // PROBLEM_H
diff --git a/include/base/processor/processor_GPS.h b/include/base/processor/processor_GPS.h
index a490bd2c1..35658410c 100644
--- a/include/base/processor/processor_GPS.h
+++ b/include/base/processor/processor_GPS.h
@@ -6,12 +6,11 @@
 #define WOLF_PROCESSOR_GPS_H
 
 // Wolf includes
-#include "processor_base.h"
-#include "capture_GPS.h"
+#include "base/processor/processor_base.h"
+#include "base/capture/capture_GPS.h"
 
 // Std includes
 
-
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(ProcessorGPS);
diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h
index 92eb67efb..98c7cffee 100644
--- a/include/base/processor/processor_IMU.h
+++ b/include/base/processor/processor_IMU.h
@@ -2,10 +2,9 @@
 #define PROCESSOR_IMU_H
 
 // Wolf
-#include "capture_IMU.h"
-#include "feature_IMU.h"
-#include "processor_motion.h"
-
+#include "base/capture/capture_IMU.h"
+#include "base/feature/feature_IMU.h"
+#include "base/processor/processor_motion.h"
 
 namespace wolf {
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index bcf5c46b6..6a1d23cfb 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,10 +7,10 @@ class SensorBase;
 }
 
 // Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
-#include "frame_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
+#include "base/frame_base.h"
 
 // std
 #include <memory>
@@ -35,8 +35,6 @@ class PackKeyFrame
 
 WOLF_PTR_TYPEDEFS(PackKeyFrame);
 
-
-
 /** \brief Buffer of Key frame class objects
  *
  * Object and functions to manage a buffer of KFPack objects.
@@ -216,8 +214,8 @@ inline void ProcessorBase::setVotingActive(bool _voting_active)
 
 }
 
-#include "sensor_base.h"
-#include "constraint_base.h"
+#include "base/sensor/sensor_base.h"
+#include "base/constraint/constraint_base.h"
 
 namespace wolf {
 
diff --git a/include/base/processor/processor_capture_holder.h b/include/base/processor/processor_capture_holder.h
index b79743f45..9067b4cae 100644
--- a/include/base/processor/processor_capture_holder.h
+++ b/include/base/processor/processor_capture_holder.h
@@ -9,9 +9,9 @@
 #define _WOLF_PROCESSOR_CAPTURE_HOLDER_H_
 
 //Wolf includes
-#include "processor_base.h"
-#include "capture_base.h"
-#include "capture_buffer.h"
+#include "base/processor/processor_base.h"
+#include "base/capture/capture_base.h"
+#include "base/capture/capture_buffer.h"
 
 namespace wolf {
 
diff --git a/src/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
similarity index 98%
rename from src/processor/processor_diff_drive.h
rename to include/base/processor/processor_diff_drive.h
index f06b5940f..e219b552f 100644
--- a/src/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -8,8 +8,8 @@
 #ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
 #define _WOLF_PROCESSOR_DIFF_DRIVE_H_
 
-#include "../processor_motion.h"
-#include "../sensors/diff_drive_tools.h"
+#include "base/processor/processor_motion.h"
+#include "base/diff_drive_tools.h"
 
 namespace wolf {
 
diff --git a/include/base/processor/processor_factory.h b/include/base/processor/processor_factory.h
index 00d2d50f6..8077aae49 100644
--- a/include/base/processor/processor_factory.h
+++ b/include/base/processor/processor_factory.h
@@ -15,7 +15,7 @@ struct ProcessorParamsBase;
 }
 
 // wolf
-#include "factory.h"
+#include "base/factory.h"
 
 // std
 
@@ -125,8 +125,8 @@ namespace wolf
  * and bind it to a SensorOdom2D:
  *
  *     \code
- *     #include "sensor_odom_2D.h"      // provides SensorOdom2D    and SensorFactory
- *     #include "processor_odom_2D.h"   // provides ProcessorOdom2D and ProcessorFactory
+ *     #include "base/sensor/sensor_odom_2D.h"      // provides SensorOdom2D    and SensorFactory
+ *     #include "base/processor/processor_odom_2D.h"   // provides ProcessorOdom2D and ProcessorFactory
  *
  *     // Note: SensorOdom2D::create()    is already registered, automatically.
  *     // Note: ProcessorOdom2D::create() is already registered, automatically.
@@ -153,9 +153,9 @@ namespace wolf
  * The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
  *
  *     \code
- *     #include "sensor_odom_2D.h"
- *     #include "processor_odom_2D.h"
- *     #include "problem.h"
+ *     #include "base/sensor/sensor_odom_2D.h"
+ *     #include "base/processor/processor_odom_2D.h"
+ *     #include "base/problem.h"
  *
  *     Problem problem(FRM_PO_2D);
  *     problem.installSensor    ( "ODOM 2D" , "Main odometer" , extrinsics      , &intrinsics );
@@ -175,7 +175,6 @@ inline std::string ProcessorFactory::getClass()
   return "ProcessorFactory";
 }
 
-
 #define WOLF_REGISTER_PROCESSOR(ProcessorType, ProcessorName) \
   namespace{ const bool WOLF_UNUSED ProcessorName##Registered = \
     wolf::ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); }\
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
index 1d9fb56b6..be1205e27 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -2,8 +2,8 @@
 #define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
 
 // Wolf related headers
-#include "processor_loopclosure_base.h"
-#include "state_block.h"
+#include "base/processor/processor_loopclosure_base.h"
+#include "base/state_block.h"
 
 namespace wolf{
 
diff --git a/include/base/processor/processor_logging.h b/include/base/processor/processor_logging.h
index 167beb892..4add7a8e9 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 "logging.h"
+//#include "base/logging.h"
 
 #define __INTERNAL_WOLF_ASSERT_PROCESSOR \
   static_assert(std::is_base_of<ProcessorBase, \
diff --git a/include/base/processor/processor_loopclosure_base.h b/include/base/processor/processor_loopclosure_base.h
index 78dd9afce..1ded95bf0 100644
--- a/include/base/processor/processor_loopclosure_base.h
+++ b/include/base/processor/processor_loopclosure_base.h
@@ -2,7 +2,7 @@
 #define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
 
 // Wolf related headers
-#include "processor_base.h"
+#include "base/processor/processor_base.h"
 
 namespace wolf{
 
@@ -35,7 +35,6 @@ class ProcessorLoopClosureBase : public ProcessorBase
 {
 protected:
 
-
   ProcessorParamsLoopClosurePtr params_loop_closure_;
 
   // Frames that are possible loop closure candidates according to
@@ -96,7 +95,6 @@ protected:
    */
   virtual void postProcess() { }
 
-
   /** Find a loop closure between incoming data and all keyframe data
    *
    * This is called by process() .
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index e399bddcc..0ce05f202 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -9,9 +9,9 @@
 #define PROCESSOR_MOTION_H_
 
 // Wolf
-#include "capture_motion.h"
-#include "processor_base.h"
-#include "time_stamp.h"
+#include "base/capture/capture_motion.h"
+#include "base/processor/processor_base.h"
+#include "base/time_stamp.h"
 
 // std
 #include <iomanip>
@@ -91,7 +91,6 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
  *
  *     \code    statePlusDelta(R_old, delta_R, R_new) \endcode
  *
- *
  * ### Defining (or not) the fromSensorFrame():
  *
  * In most cases, one will be interested in avoiding the \b fromSensorFrame() issue.
@@ -201,7 +200,6 @@ class ProcessorMotion : public ProcessorBase
         MotionBuffer& getBuffer();
         const MotionBuffer& getBuffer() const;
 
-
         // Helper functions:
     protected:
 
@@ -237,11 +235,9 @@ class ProcessorMotion : public ProcessorBase
 
         PackKeyFramePtr computeProcessingStep();
 
-
         // These are the pure virtual functions doing the mathematics
     protected:
 
-
         /** \brief convert raw CaptureMotion data to the delta-state format
          *
          * This function accepts raw data and time step dt,
@@ -355,7 +351,6 @@ class ProcessorMotion : public ProcessorBase
          */
         virtual Eigen::VectorXs deltaZero() const = 0;
 
-
         /** \brief Interpolate motion to an intermediate time-stamp
          *
          * @param _ref    The motion reference
@@ -449,8 +444,6 @@ class ProcessorMotion : public ProcessorBase
         void setDistTraveled(const Scalar& _dist_traveled);
         void setAngleTurned(const Scalar& _angle_turned);
 
-
-
     protected:
         // Attributes
         SizeEigen x_size_;           ///< The size of the state vector
@@ -481,7 +474,7 @@ class ProcessorMotion : public ProcessorBase
 
 }
 
-#include "frame_base.h"
+#include "base/frame_base.h"
 
 namespace wolf{
 
@@ -558,7 +551,6 @@ inline const MotionBuffer& ProcessorMotion::getBuffer() const
     return last_ptr_->getBuffer();
 }
 
-
 inline MotionBuffer& ProcessorMotion::getBuffer()
 {
     return last_ptr_->getBuffer();
@@ -631,8 +623,6 @@ inline void ProcessorMotion::setAngleTurned(const Scalar& _angle_turned)
     params_motion_->angle_turned = _angle_turned;
 }
 
-
-
 } // namespace wolf
 
 #endif /* PROCESSOR_MOTION2_H_ */
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index bf797ca8b..b7ea895df 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -8,11 +8,10 @@
 #ifndef SRC_PROCESSOR_ODOM_2D_H_
 #define SRC_PROCESSOR_ODOM_2D_H_
 
-#include "processor_motion.h"
-#include "capture_odom_2D.h"
-#include "constraint_odom_2D.h"
-#include "rotations.h"
-
+#include "base/processor/processor_motion.h"
+#include "base/capture/capture_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/rotations.h"
 
 namespace wolf {
     
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 9a9828d4b..77766efc3 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -8,14 +8,13 @@
 #ifndef SRC_PROCESSOR_ODOM_3D_H_
 #define SRC_PROCESSOR_ODOM_3D_H_
 
-#include "processor_motion.h"
-#include "sensor_odom_3D.h"
-#include "capture_odom_3D.h"
-#include "constraint_odom_3D.h"
-#include "rotations.h"
+#include "base/processor/processor_motion.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/capture/capture_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/rotations.h"
 #include <cmath>
 
-
 namespace wolf {
     
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
@@ -25,7 +24,6 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion
         //
 };
 
-
 WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
 
 /** \brief Processor for 3d odometry integration.
diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h
index fde468176..6ce3eaace 100644
--- a/include/base/processor/processor_params_image.h
+++ b/include/base/processor/processor_params_image.h
@@ -2,8 +2,8 @@
 #define PROCESSOR_IMAGE_PARAMS_H
 
 // wolf
-#include "processor_tracker_feature.h"
-#include "processor_tracker_landmark.h"
+#include "base/processor/processor_tracker_feature.h"
+#include "base/processor/processor_tracker_landmark.h"
 
 namespace wolf
 {
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index 5d18402b6..f132626d7 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -8,8 +8,8 @@
 #ifndef SRC_PROCESSOR_TRACKER_H_
 #define SRC_PROCESSOR_TRACKER_H_
 
-#include "processor_base.h"
-#include "capture_motion.h"
+#include "base/processor/processor_base.h"
+#include "base/capture/capture_motion.h"
 
 namespace wolf {
     
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index fc35c5465..52bd493f6 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -9,11 +9,11 @@
 #define PROCESSOR_TRACKER_FEATURE_H_
 
 //wolf includes
-#include "processor_tracker.h"
-#include "capture_base.h"
-#include "feature_match.h"
-#include "track_matrix.h"
-#include "wolf.h"
+#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"
 
 namespace wolf
 {
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index efc40a145..5bc33bf47 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -9,14 +9,14 @@
 #define PROCESSOR_TRACKER_FEATURE_CORNER_H_
 
 // Wolf includes
-#include "sensor_laser_2D.h"
-#include "capture_laser_2D.h"
-#include "feature_corner_2D.h"
-#include "landmark_corner_2D.h"
-#include "constraint_corner_2D.h"
-#include "state_block.h"
-#include "data_association/association_tree.h"
-#include "processor_tracker_feature.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
+#include "base/feature/feature_corner_2D.h"
+#include "base/landmark/landmark_corner_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
+#include "base/state_block.h"
+#include "base/association/association_tree.h"
+#include "base/processor/processor_tracker_feature.h"
 
 //laser_scan_utils
 //#include "laser_scan_utils/scan_basics.h"
@@ -25,7 +25,6 @@
 #include "laser_scan_utils/line_finder_iterative.h"
 #include "laser_scan_utils/corner_finder.h"
 
-
 namespace wolf
 {
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureCorner);
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 3c163d276..dbc024b7c 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -8,9 +8,9 @@
 #ifndef PROCESSOR_TRACKER_FEATURE_DUMMY_H_
 #define PROCESSOR_TRACKER_FEATURE_DUMMY_H_
 
-#include "wolf.h"
-#include "processor_tracker_feature.h"
-#include "constraint_epipolar.h"
+#include "base/wolf.h"
+#include "base/processor/processor_tracker_feature.h"
+#include "base/constraint/constraint_epipolar.h"
 
 namespace wolf
 {
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 792c5d645..48f7e7a70 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -2,14 +2,14 @@
 #define PROCESSOR_TRACKER_FEATURE_IMAGE_H
 
 // Wolf includes
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "feature_point_image.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_tracker_feature.h"
-#include "constraint_epipolar.h"
-#include "processor_params_image.h"
+#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/processor/processor_tracker_feature.h"
+#include "base/constraint/constraint_epipolar.h"
+#include "base/processor/processor_params_image.h"
 
 // vision_utils
 #include <detectors/detector_base.h>
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index bae94066b..293570afa 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -9,9 +9,9 @@
 #define PROCESSOR_TRACKER_LANDMARK_H_
 
 //wolf includes
-#include "processor_tracker.h"
-#include "capture_base.h"
-#include "landmark_match.h"
+#include "base/processor/processor_tracker.h"
+#include "base/capture/capture_base.h"
+#include "base/landmark/landmark_match.h"
 
 namespace wolf
 {
@@ -23,7 +23,6 @@ struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker
     //
 };
 
-
 WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
     
 /** \brief Landmark tracker processor
@@ -168,6 +167,6 @@ class ProcessorTrackerLandmark : public ProcessorTracker
 }// namespace wolf
 
 // IMPLEMENTATION
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
 
 #endif /* PROCESSOR_TRACKER_LANDMARK_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index bb9eff8db..bd81b1643 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -9,14 +9,14 @@
 #define SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_
 
 // Wolf includes
-#include "sensor_laser_2D.h"
-#include "capture_laser_2D.h"
-#include "feature_corner_2D.h"
-#include "landmark_corner_2D.h"
-#include "constraint_corner_2D.h"
-#include "state_block.h"
-#include "data_association/association_tree.h"
-#include "processor_tracker_landmark.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
+#include "base/feature/feature_corner_2D.h"
+#include "base/landmark/landmark_corner_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
+#include "base/state_block.h"
+#include "base/association/association_tree.h"
+#include "base/processor/processor_tracker_landmark.h"
 
 //laser_scan_utils
 //#include "laser_scan_utils/scan_basics.h"
@@ -34,7 +34,6 @@ const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
 const Scalar position_error_th_ = 1;
 const Scalar min_features_ratio_th_ = 0.5;
 
-
 //forward declaration to typedef class pointers
 struct ProcessorParamsLaser;
 typedef std::shared_ptr<ProcessorParamsLaser> ProcessorParamsLaserPtr;
@@ -62,7 +61,6 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         laserscanutils::CornerFinder corner_finder_;
         //TODO: add corner_finder_params
 
-
         FeatureBaseList corners_incoming_;
         FeatureBaseList corners_last_;
         unsigned int new_corners_th_;
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index 410b9c72c..1d9d66bba 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -8,7 +8,7 @@
 #ifndef PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
 #define PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
 
-#include "processor_tracker_landmark.h"
+#include "base/processor/processor_tracker_landmark.h"
 
 namespace wolf
 {
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index 6d86249e7..1b7a0dbd6 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -3,11 +3,11 @@
 
 // Wolf includes
 
-#include "landmark_AHP.h"
-#include "landmark_match.h"
-#include "processor_params_image.h"
-#include "processor_tracker_landmark.h"
-#include "wolf.h"
+#include "base/landmark/landmark_AHP.h"
+#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 <algorithms/activesearch/alg_activesearch.h>
 #include <descriptors/descriptor_base.h>
@@ -102,7 +102,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
             image_last_ = image_incoming_;
         }
 
-
         /**
          * \brief Does the drawing of the features.
          *
@@ -147,8 +146,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
     public:
         static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
 
-
-
         /** \brief Create a new constraint
          * \param _feature_ptr pointer to the Feature to constrain
          * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
@@ -190,8 +187,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
 
 };
 
-
 } // namespace wolf
 
-
 #endif // PROCESSOR_TRACKER_LANDMARK_IMAGE_H
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
index 3c4c69031..e8d70824a 100644
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ b/include/base/processor/processor_tracker_landmark_polyline.h
@@ -9,15 +9,15 @@
 #define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
 
 // Wolf includes
-#include "sensor_laser_2D.h"
-#include "capture_laser_2D.h"
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
-#include "constraint_point_2D.h"
-#include "constraint_point_to_line_2D.h"
-#include "state_block.h"
-#include "data_association/association_tree.h"
-#include "processor_tracker_landmark.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/constraint/constraint_point_2D.h"
+#include "base/constraint/constraint_point_to_line_2D.h"
+#include "base/state_block.h"
+#include "base/association/association_tree.h"
+#include "base/processor/processor_tracker_landmark.h"
 
 //laser_scan_utils
 #include "laser_scan_utils/laser_scan.h"
@@ -31,7 +31,6 @@ namespace wolf
 const Scalar position_error_th_ = 1;
 const Scalar min_features_ratio_th_ = 0.5;
 
-
 //forward declaration to typedef class pointers
 struct LandmarkPolylineMatch;
 typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
@@ -40,10 +39,8 @@ typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
 struct ProcessorParamsPolyline;
 typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
 
-
 WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
 
-
 // Match Feature - Landmark
 struct LandmarkPolylineMatch : public LandmarkMatch
 {
diff --git a/include/base/rotations.h b/include/base/rotations.h
index 2ce225537..d4d3e711f 100644
--- a/include/base/rotations.h
+++ b/include/base/rotations.h
@@ -8,7 +8,7 @@
 #ifndef ROTATIONS_H_
 #define ROTATIONS_H_
 
-#include "wolf.h"
+#include "base/wolf.h"
 
 namespace wolf
 {
@@ -96,7 +96,6 @@ vee(const Eigen::MatrixBase<Derived>& _m)
     return (Eigen::Matrix<T, 3, 1>() << _m(2, 1), _m(0, 2), _m(1, 0)).finished();
 }
 
-
 ////////////////////////////////////////////////////
 // Rotation conversions q, R, Euler, back and forth
 //
@@ -161,7 +160,6 @@ e2q(const Eigen::MatrixBase<D>& _euler)
     return Eigen::Quaternion<T>(az * ay * ax);
 }
 
-
 /** \brief Euler angles to rotation matrix
  * \param _e = (roll pitch yaw) in ZYX convention
  * \return equivalent rotation matrix
@@ -175,7 +173,6 @@ e2R(const Eigen::MatrixBase<Derived>& _e)
     return e2q(_e).matrix();
 }
 
-
 template <typename Derived>
 inline typename Eigen::MatrixBase<Derived>::Scalar
 getYaw(const Eigen::MatrixBase<Derived>& R)
@@ -199,7 +196,6 @@ R2e(const Eigen::MatrixBase<Derived>& _R)
     return e;
 }
 
-
 template<typename Derived>
 inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
 q2e(const Eigen::MatrixBase<Derived>& _q)
@@ -235,7 +231,6 @@ q2e(const Eigen::MatrixBase<Derived>& _q)
     return e;
 }
 
-
 template<typename Derived>
 inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
 q2e(const Eigen::QuaternionBase<Derived>& _q)
@@ -243,9 +238,6 @@ q2e(const Eigen::QuaternionBase<Derived>& _q)
     return q2e(_q.coeffs());
 }
 
-
-
-
 ///////////////////////////////////////////////////////////////
 // Rotation conversions - exp and log maps
 
@@ -295,7 +287,6 @@ log_q(const Eigen::QuaternionBase<Derived>& _q)
     //    Eigen::AngleAxis<T> aa(_q);
     //    return aa.angle() * aa.axis();
 
-
     // In the meanwhile, we have a custom implementation as follows
 
     typedef typename Derived::Scalar T;
@@ -411,8 +402,6 @@ R2v(const Eigen::MatrixBase<Derived>& _R)
     return log_R(_R);
 }
 
-
-
 /////////////////////////////////////////////////////////////////
 // Jacobians of SO(3)
 
@@ -651,7 +640,6 @@ diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
     return minus(q1, q2);
 }
 
-
 } // namespace wolf
 
 #endif /* ROTATIONS_H_ */
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index c43cd13cb..17b6f2033 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -9,9 +9,9 @@
  */
 
 //wolf
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 //#include "sensor_factory.h"
-//#include "factory.h"
+//#include "base/factory.h"
 
 // std
 
@@ -40,7 +40,6 @@ public:
 public:
     static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
 
-
 };
 
 } // namespace wolf
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
index c319d159f..e74ff5e89 100644
--- a/include/base/sensor/sensor_GPS_fix.h
+++ b/include/base/sensor/sensor_GPS_fix.h
@@ -3,13 +3,10 @@
 #define SENSOR_GPS_FIX_H_
 
 //wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 // std includes
 
-
-
-
 namespace wolf {
 
 WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 2f5d9baa5..17c1c873d 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/sensor_IMU.h
@@ -2,7 +2,7 @@
 #define SENSOR_IMU_H
 
 //wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 #include <iostream>
 
 namespace wolf {
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index b52e8816e..8707d13ac 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -9,15 +9,14 @@ class StateBlock;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-#include "time_stamp.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
+#include "base/time_stamp.h"
 
 //std includes
 
 namespace wolf {
 
-
 /** \brief base struct for intrinsic sensor parameters
  *
  * Derive from this struct to create structs of sensor intrinsic parameters.
@@ -162,10 +161,10 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
 
 }
 
-#include "problem.h"
-#include "hardware_base.h"
-#include "capture_base.h"
-#include "processor_base.h"
+#include "base/problem.h"
+#include "base/hardware_base.h"
+#include "base/capture/capture_base.h"
+#include "base/processor/processor_base.h"
 
 namespace wolf{
 
@@ -273,8 +272,6 @@ inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
     intrinsic_dynamic_ = _intrinsic_dynamic;
 }
 
-
-
 } // namespace wolf
 
 #endif
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index d8641510d..2214fc14a 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -2,7 +2,7 @@
 #define SENSOR_CAMERA_H
 
 //wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 namespace wolf
 {
@@ -20,7 +20,6 @@ struct IntrinsicsCamera : public IntrinsicsBase
         virtual ~IntrinsicsCamera() = default;
 };
 
-
 WOLF_PTR_TYPEDEFS(SensorCamera);
 /**Pin-hole camera sensor
  */
diff --git a/src/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
similarity index 97%
rename from src/sensor/sensor_diff_drive.h
rename to include/base/sensor/sensor_diff_drive.h
index 925c840e5..0f5cf4259 100644
--- a/src/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -9,8 +9,8 @@
 #define WOLF_SENSOR_DIFF_DRIVE_H_
 
 //wolf includes
-#include "../sensor_base.h"
-#include "diff_drive_tools.h"
+#include "base/sensor/sensor_base.h"
+#include "base/diff_drive_tools.h"
 
 namespace wolf {
 
diff --git a/include/base/sensor/sensor_factory.h b/include/base/sensor/sensor_factory.h
index 3e7e1aabc..32884c18f 100644
--- a/include/base/sensor/sensor_factory.h
+++ b/include/base/sensor/sensor_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
 }
 
 // wolf
-#include "factory.h"
+#include "base/factory.h"
 
 namespace wolf
 {
@@ -174,7 +174,7 @@ namespace wolf
  *
  *  \code
  *  #include "sensor_factory.h"
- *  #include "sensor_camera.h" // provides SensorCamera
+ *  #include "base/sensor/sensor_camera.h" // provides SensorCamera
  *
  *  // Note: SensorCamera::create() is already registered, automatically.
  *
diff --git a/include/base/sensor/sensor_laser_2D.h b/include/base/sensor/sensor_laser_2D.h
index 9cdcf5567..6b927c5b4 100644
--- a/include/base/sensor/sensor_laser_2D.h
+++ b/include/base/sensor/sensor_laser_2D.h
@@ -4,7 +4,7 @@
 #define SENSOR_LASER_2D_H_
 
 //wolf
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 //laser_scan_utils
 #include "laser_scan_utils/laser_scan.h"
diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
index 4ba0fc37d..75110b2cd 100644
--- a/include/base/sensor/sensor_odom_2D.h
+++ b/include/base/sensor/sensor_odom_2D.h
@@ -3,7 +3,7 @@
 #define SENSOR_ODOM_2D_H_
 
 //wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 namespace wolf {
 
@@ -59,7 +59,6 @@ class SensorOdom2D : public SensorBase
 	public:
         static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
 
-
 };
 
 } // namespace wolf
diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
index 19f037162..76c9a3d5b 100644
--- a/include/base/sensor/sensor_odom_3D.h
+++ b/include/base/sensor/sensor_odom_3D.h
@@ -9,7 +9,7 @@
 #define SRC_SENSOR_ODOM_3D_H_
 
 //wolf includes
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 namespace wolf {
 
@@ -60,7 +60,6 @@ class SensorOdom3D : public SensorBase
     public:
         static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
 
-
 };
 
 inline Scalar SensorOdom3D::getDispVarToDispNoiseFactor() const
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 9f1189495..4801fd57f 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -2,9 +2,9 @@
 #define _WOLF_SOLVER_MANAGER_H_
 
 //wolf includes
-#include "../wolf.h"
-#include "../state_block.h"
-#include "../constraint_base.h"
+#include "base/wolf.h"
+#include "base/state_block.h"
+#include "base/constraint/constraint_base.h"
 
 namespace wolf {
 
diff --git a/include/base/solver_suitesparse/ccolamd_ordering.h b/include/base/solver_suitesparse/ccolamd_ordering.h
index d88c0d41b..bae2eff4d 100644
--- a/include/base/solver_suitesparse/ccolamd_ordering.h
+++ b/include/base/solver_suitesparse/ccolamd_ordering.h
@@ -8,8 +8,6 @@
 #ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
 #define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
 
-
-
 //std includes
 #include <iostream>
 
@@ -79,5 +77,4 @@ class CCOLAMDOrdering
 };
 }
 
-
 #endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ */
diff --git a/include/base/solver_suitesparse/cost_function_base.h b/include/base/solver_suitesparse/cost_function_base.h
index 76ba74da3..8aae707ae 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 "wolf.h"
+#include "base/wolf.h"
 #include <Eigen/StdVector>
 
 class CostFunctionBase
@@ -75,7 +75,6 @@ class CostFunctionBase
 
         virtual void evaluateResidualJacobians() = 0;
 
-
         void getResidual(Eigen::VectorXs &residual)
         {
             residual.resize(residual_.size());
diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h
index 37033bcb9..713e29bd3 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 "wolf.h"
+#include "base/wolf.h"
 #include "cost_function_sparse_base.h"
 
 // CERES JET
@@ -229,7 +229,6 @@ class CostFunctionSparse<ConstraintT,
                                     BLOCK_5_SIZE,
                                     BLOCK_6_SIZE,
                                     0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
@@ -240,7 +239,6 @@ class CostFunctionSparse<ConstraintT,
                                                         BLOCK_5_SIZE,
                                                         BLOCK_6_SIZE,
                                                         0,
-                                                        0,
                                                         0>
 {
     public:
@@ -255,7 +253,6 @@ class CostFunctionSparse<ConstraintT,
                                 BLOCK_5_SIZE,
                                 BLOCK_6_SIZE,
                                 0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
@@ -284,8 +281,6 @@ class CostFunctionSparse<ConstraintT,
                                     BLOCK_4_SIZE,
                                     BLOCK_5_SIZE,
                                     0,
-                                    0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
@@ -295,8 +290,6 @@ class CostFunctionSparse<ConstraintT,
                                                         BLOCK_4_SIZE,
                                                         BLOCK_5_SIZE,
                                                         0,
-                                                        0,
-                                                        0,
                                                         0>
 {
     public:
@@ -310,8 +303,6 @@ class CostFunctionSparse<ConstraintT,
                                 BLOCK_4_SIZE,
                                 BLOCK_5_SIZE,
                                 0,
-                                0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
@@ -338,9 +329,6 @@ class CostFunctionSparse<ConstraintT,
                                     BLOCK_3_SIZE,
                                     BLOCK_4_SIZE,
                                     0,
-                                    0,
-                                    0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
@@ -349,9 +337,6 @@ class CostFunctionSparse<ConstraintT,
                                                         BLOCK_3_SIZE,
                                                         BLOCK_4_SIZE,
                                                         0,
-                                                        0,
-                                                        0,
-                                                        0,
                                                         0>
 {
     public:
@@ -364,9 +349,6 @@ class CostFunctionSparse<ConstraintT,
                                 BLOCK_3_SIZE,
                                 BLOCK_4_SIZE,
                                 0,
-                                0,
-                                0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
@@ -391,10 +373,6 @@ class CostFunctionSparse<ConstraintT,
                                     BLOCK_2_SIZE,
                                     BLOCK_3_SIZE,
                                     0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
@@ -402,10 +380,6 @@ class CostFunctionSparse<ConstraintT,
                                                         BLOCK_2_SIZE,
                                                         BLOCK_3_SIZE,
                                                         0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
                                                         0>
 {
     public:
@@ -417,10 +391,6 @@ class CostFunctionSparse<ConstraintT,
                                 BLOCK_2_SIZE,
                                 BLOCK_3_SIZE,
                                 0,
-                                0,
-                                0,
-                                0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
@@ -443,22 +413,12 @@ class CostFunctionSparse<ConstraintT,
                                     BLOCK_1_SIZE,
                                     BLOCK_2_SIZE,
                                     0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
                                                         BLOCK_1_SIZE,
                                                         BLOCK_2_SIZE,
                                                         0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
                                                         0>
 {
     public:
@@ -469,11 +429,6 @@ class CostFunctionSparse<ConstraintT,
                                 BLOCK_1_SIZE,
                                 BLOCK_2_SIZE,
                                 0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
@@ -494,23 +449,11 @@ class CostFunctionSparse<ConstraintT,
                                     BLOCK_0_SIZE,
                                     BLOCK_1_SIZE,
                                     0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
                                                         BLOCK_1_SIZE,
                                                         0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
                                                         0>
 {
     public:
@@ -520,12 +463,6 @@ class CostFunctionSparse<ConstraintT,
                                 BLOCK_0_SIZE,
                                 BLOCK_1_SIZE,
                                 0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
@@ -544,24 +481,10 @@ class CostFunctionSparse<ConstraintT,
                                     MEASUREMENT_SIZE,
                                     BLOCK_0_SIZE,
                                     0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
-                                    0,
                                     0> : CostFunctionSparseBase<ConstraintT,
                                                         MEASUREMENT_SIZE,
                                                         BLOCK_0_SIZE,
                                                         0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
-                                                        0,
                                                         0>
 {
     public:
@@ -570,13 +493,6 @@ class CostFunctionSparse<ConstraintT,
                                 MEASUREMENT_SIZE,
                                 BLOCK_0_SIZE,
                                 0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
                                 0>(_constraint_ptr)
         {
 
diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h
index 4b54001f6..d10fbab58 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 "wolf.h"
+#include "base/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 493ee2dc8..6d032e8b2 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -13,12 +13,12 @@
 #include <ctime>
 
 //Wolf includes
-#include "state_block.h"
+#include "base/state_block.h"
 #include "../constraint_sparse.h"
-#include "../constraint_odom_2D.h"
-#include "../constraint_corner_2D.h"
-#include "../constraint_container.h"
-#include "sparse_utils.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
+#include "base/constraint/constraint_container.h"
+#include "base/solver_suitesparse/sparse_utils.h"
 
 // wolf solver
 #include "solver/ccolamd_ordering.h"
@@ -28,7 +28,7 @@
 #include <eigen3/Eigen/OrderingMethods>
 #include <eigen3/Eigen/SparseQR>
 #include <Eigen/StdVector>
-#include "../constraint_pose_2D.h"
+#include "base/constraint/constraint_pose_2D.h"
 
 namespace wolf
 {
diff --git a/include/base/solver_suitesparse/solver_QR.h b/include/base/solver_suitesparse/solver_QR.h
index 2fa0d11d1..9451ceec9 100644
--- a/include/base/solver_suitesparse/solver_QR.h
+++ b/include/base/solver_suitesparse/solver_QR.h
@@ -29,5 +29,4 @@ class SolverQR
     private:
 };
 
-
 #endif /* TRUNK_SRC_SOLVER_SOLVER_QR_H_ */
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
index b9084d4fb..479cc4509 100644
--- a/include/base/solver_suitesparse/solver_manager.h
+++ b/include/base/solver_suitesparse/solver_manager.h
@@ -2,9 +2,9 @@
 #define CERES_MANAGER_H_
 
 //wolf includes
-#include "../constraint_GPS_2D.h"
-#include "../wolf.h"
-#include "../state_block.h"
+#include "base/constraint/constraint_GPS_2D.h"
+#include "base/wolf.h"
+#include "base/state_block.h"
 #include "../state_point.h"
 #include "../state_complex_angle.h"
 #include "../state_theta.h"
@@ -21,7 +21,6 @@ class SolverManager
 {
 	protected:
 
-
 	public:
 		SolverManager(ceres::Problem::Options _options);
 
diff --git a/include/base/state_angle.h b/include/base/state_angle.h
index 81da7a882..c61286e13 100644
--- a/include/base/state_angle.h
+++ b/include/base/state_angle.h
@@ -8,8 +8,8 @@
 #ifndef STATE_ANGLE_H_
 #define STATE_ANGLE_H_
 
-#include "state_block.h"
-#include "local_parametrization_angle.h"
+#include "base/state_block.h"
+#include "base/local_parametrization_angle.h"
 
 namespace wolf
 {
diff --git a/include/base/state_block.h b/include/base/state_block.h
index cdfc94c24..06962ced0 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block.h
@@ -9,7 +9,7 @@ class LocalParametrizationBase;
 }
 
 //Wolf includes
-#include "wolf.h"
+#include "base/wolf.h"
 
 //std includes
 #include <iostream>
@@ -154,9 +154,9 @@ public:
 } // namespace wolf
 
 // IMPLEMENTATION
-#include "local_parametrization_base.h"
-#include "node_base.h"
-#include "problem.h"
+#include "base/local_parametrization_base.h"
+#include "base/node_base.h"
+#include "base/problem.h"
 
 namespace wolf {
 
diff --git a/include/base/state_homogeneous_3D.h b/include/base/state_homogeneous_3D.h
index f8d4951a8..adfda9018 100644
--- a/include/base/state_homogeneous_3D.h
+++ b/include/base/state_homogeneous_3D.h
@@ -8,8 +8,8 @@
 #ifndef SRC_STATE_HOMOGENEOUS_3D_H_
 #define SRC_STATE_HOMOGENEOUS_3D_H_
 
-#include "state_block.h"
-#include "local_parametrization_homogeneous.h"
+#include "base/state_block.h"
+#include "base/local_parametrization_homogeneous.h"
 
 namespace wolf {
 
diff --git a/include/base/state_quaternion.h b/include/base/state_quaternion.h
index b68fc574e..d990ce1f8 100644
--- a/include/base/state_quaternion.h
+++ b/include/base/state_quaternion.h
@@ -8,8 +8,8 @@
 #ifndef SRC_STATE_QUATERNION_H_
 #define SRC_STATE_QUATERNION_H_
 
-#include "state_block.h"
-#include "local_parametrization_quaternion.h"
+#include "base/state_block.h"
+#include "base/local_parametrization_quaternion.h"
 
 namespace wolf {
 
diff --git a/include/base/time_stamp.h b/include/base/time_stamp.h
index d38d25b5f..02a5599af 100644
--- a/include/base/time_stamp.h
+++ b/include/base/time_stamp.h
@@ -3,7 +3,7 @@
 #define TIME_STAMP_H_
 
 //wolf includes
-#include "wolf.h"
+#include "base/wolf.h"
 
 //C, std
 #include <sys/time.h>
diff --git a/include/base/track_matrix.h b/include/base/track_matrix.h
index 6f32d4cdf..5aa6159c4 100644
--- a/include/base/track_matrix.h
+++ b/include/base/track_matrix.h
@@ -8,8 +8,8 @@
 #ifndef TRACK_MATRIX_H_
 #define TRACK_MATRIX_H_
 
-#include "feature_base.h"
-#include "capture_base.h"
+#include "base/feature/feature_base.h"
+#include "base/capture/capture_base.h"
 
 #include <map>
 #include <vector>
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index cb394c5bb..4e8d261d3 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory_base.h
@@ -10,8 +10,8 @@ class TimeStamp;
 }
 
 //Wolf includes
-#include "wolf.h"
-#include "node_base.h"
+#include "base/wolf.h"
+#include "base/node_base.h"
 
 //std includes
 
@@ -71,13 +71,11 @@ inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
     last_key_frame_ptr_ = _key_frame_ptr;
 }
 
-
 inline std::string TrajectoryBase::getFrameStructure() const
 {
     return frame_structure_;
 }
 
-
 } // namespace wolf
 
 #endif
diff --git a/include/base/wolf.h b/include/base/wolf.h
index bb5439030..d642f730a 100644
--- a/include/base/wolf.h
+++ b/include/base/wolf.h
@@ -10,7 +10,7 @@
 
 // Enable project-specific definitions and macros
 #include "internal/config.h"
-#include "logging.h"
+#include "base/logging.h"
 
 //includes from Eigen lib
 #include <Eigen/Dense>
@@ -45,7 +45,6 @@ namespace wolf {
 typedef double Scalar;        // Use this for double, 64 bit precision
 //typedef long double Scalar;   // Use this for long double, 128 bit precision
 
-
 /**
  * \brief Vector and Matrices size type for the Wolf project
  *
@@ -205,7 +204,6 @@ struct MatrixSizeCheck
 //
 // End of check matrix sizes /////////////////////////////////////////////////
 
-
 /////////////////////////////////////////////////////////////////////////
 //      TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
 /////////////////////////////////////////////////////////////////////////
@@ -227,7 +225,6 @@ struct MatrixSizeCheck
         typedef std::shared_ptr<StructName>          StructName##Ptr; \
         typedef std::shared_ptr<const StructName>    StructName##ConstPtr; \
 
-
 // NodeBase
 WOLF_PTR_TYPEDEFS(NodeBase);
 
@@ -289,7 +286,6 @@ WOLF_PTR_TYPEDEFS(StateQuaternion);
 // - - Local Parametrization
 WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
 
-
 // ==================================================
 // Some dangling functions
 
@@ -373,6 +369,4 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
 
 } // namespace wolf
 
-
-
 #endif /* WOLF_H_ */
diff --git a/serialization/cereal/io.h b/serialization/cereal/io.h
index 524af2e41..8a8e58c2d 100644
--- a/serialization/cereal/io.h
+++ b/serialization/cereal/io.h
@@ -167,7 +167,6 @@ using SerializerJSON = Serializer<Extensions::JSON,
                                   cereal::JSONInputArchive,
                                   cereal::JSONOutputArchive>;
 
-
 } /* namespace serialization */
 
 template <typename... T>
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
index b57b9f564..0c38a5a61 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 "../../local_parametrization_base.h"
+#include "base/local_parametrization_base.h"
 
 #include <cereal/cereal.hpp>
 #include <cereal/types/polymorphic.hpp>
@@ -21,7 +21,6 @@ inline void serialize(
   //
 }
 
-
 } //namespace cereal
 
 #endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_homogeneous.h b/serialization/cereal/serialization_local_parametrization_homogeneous.h
index aa04a9904..2b36232d5 100644
--- a/serialization/cereal/serialization_local_parametrization_homogeneous.h
+++ b/serialization/cereal/serialization_local_parametrization_homogeneous.h
@@ -1,7 +1,7 @@
 #ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
 #define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
 
-#include "../../local_parametrization_homogeneous.h"
+#include "base/local_parametrization_homogeneous.h"
 
 #include "serialization_local_parametrization_base.h"
 
diff --git a/serialization/cereal/serialization_local_parametrization_quaternion.h b/serialization/cereal/serialization_local_parametrization_quaternion.h
index 10b04f67c..95c71f4c2 100644
--- a/serialization/cereal/serialization_local_parametrization_quaternion.h
+++ b/serialization/cereal/serialization_local_parametrization_quaternion.h
@@ -1,7 +1,7 @@
 #ifndef WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
 #define WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
 
-#include "../../local_parametrization_quaternion.h"
+#include "base/local_parametrization_quaternion.h"
 
 #include "serialization_local_parametrization_base.h"
 
diff --git a/serialization/cereal/serialization_node_base.h b/serialization/cereal/serialization_node_base.h
index dbf3292d5..5a0a039a3 100644
--- a/serialization/cereal/serialization_node_base.h
+++ b/serialization/cereal/serialization_node_base.h
@@ -2,7 +2,7 @@
 #define _WOLF_IO_CEREAL_NODE_BASE_H_
 
 // Wolf includes
-#include "../../node_base.h"
+#include "base/node_base.h"
 
 #include <cereal/cereal.hpp>
 #include <cereal/types/polymorphic.hpp>
diff --git a/serialization/cereal/serialization_processor_odom2d_params.h b/serialization/cereal/serialization_processor_odom2d_params.h
index 4e34becad..2056f6de9 100644
--- a/serialization/cereal/serialization_processor_odom2d_params.h
+++ b/serialization/cereal/serialization_processor_odom2d_params.h
@@ -2,7 +2,7 @@
 #define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
 
 // Wolf includes
-#include "../../processor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
 #include "serialization_processor_params_base.h"
 
 namespace cereal {
diff --git a/serialization/cereal/serialization_processor_odom3d_params.h b/serialization/cereal/serialization_processor_odom3d_params.h
index 9b658a233..1556a39e4 100644
--- a/serialization/cereal/serialization_processor_odom3d_params.h
+++ b/serialization/cereal/serialization_processor_odom3d_params.h
@@ -2,7 +2,7 @@
 #define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
 
 // Wolf includes
-#include "../../processor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
 #include "serialization_processor_params_base.h"
 
 namespace cereal {
diff --git a/serialization/cereal/serialization_processor_params_base.h b/serialization/cereal/serialization_processor_params_base.h
index 03cbce782..2c75b85c9 100644
--- a/serialization/cereal/serialization_processor_params_base.h
+++ b/serialization/cereal/serialization_processor_params_base.h
@@ -2,7 +2,7 @@
 #define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
 
 // Wolf includes
-#include "../../processor_base.h"
+#include "base/processor/processor_base.h"
 
 #include <cereal/cereal.hpp>
 #include <cereal/types/polymorphic.hpp>
diff --git a/serialization/cereal/serialization_sensor_intrinsic_base.h b/serialization/cereal/serialization_sensor_intrinsic_base.h
index 2398fbad0..3e3b1cdb4 100644
--- a/serialization/cereal/serialization_sensor_intrinsic_base.h
+++ b/serialization/cereal/serialization_sensor_intrinsic_base.h
@@ -2,7 +2,7 @@
 #define _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
 
 // Wolf includes
-#include "../../sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 #include <cereal/cereal.hpp>
 #include <cereal/types/polymorphic.hpp>
diff --git a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
index d4bf37044..ccfe569a7 100644
--- a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
+++ b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
@@ -2,7 +2,7 @@
 #define _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
 
 // Wolf includes
-#include "../../sensor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
 
 #include "serialization_sensor_intrinsic_base.h"
 
diff --git a/serialization/cereal/serialization_time_stamp.h b/serialization/cereal/serialization_time_stamp.h
index 770e622f0..6cc9e29c7 100644
--- a/serialization/cereal/serialization_time_stamp.h
+++ b/serialization/cereal/serialization_time_stamp.h
@@ -2,7 +2,7 @@
 #define _WOLF_IO_CEREAL_TIME_STAMP_H_
 
 // Wolf includes
-#include "../../time_stamp.h"
+#include "base/time_stamp.h"
 
 #include <cereal/cereal.hpp>
 
diff --git a/src/SE3.h b/src/SE3.h
index cecd71757..cea354607 100644
--- a/src/SE3.h
+++ b/src/SE3.h
@@ -8,9 +8,8 @@
 #ifndef SE3_H_
 #define SE3_H_
 
-
-#include "wolf.h"
-#include "rotations.h"
+#include "base/wolf.h"
+#include "base/rotations.h"
 
 /*
  * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
@@ -34,8 +33,6 @@
  *   - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
  */
 
-
-
 namespace wolf
 {
 namespace three_D {
@@ -95,7 +92,6 @@ inline void inverse(const MatrixBase<D1>& d,
     inverse(dp, dq, idp, idq);
 }
 
-
 template<typename D>
 inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
 {
@@ -208,7 +204,6 @@ inline void between(const MatrixBase<D1>& d1,
     between(dp1, dq1, dp2, dq2, dp12, dq12);
 }
 
-
 template<typename D1, typename D2>
 inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
                                                  const MatrixBase<D2>& d2 )
@@ -320,7 +315,6 @@ inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
     J_do_dq2    =   jac_SO3_right_inv(diff_o);
 }
 
-
 template<typename D1, typename D2, typename D3>
 inline void minus(const MatrixBase<D1>& d1,
                   const MatrixBase<D2>& d2,
diff --git a/src/association/association_nnls.cpp b/src/association/association_nnls.cpp
index 0c42108ea..5d8286a78 100644
--- a/src/association/association_nnls.cpp
+++ b/src/association/association_nnls.cpp
@@ -1,4 +1,4 @@
-#include "association_nnls.h"
+#include "base/association/association_nnls.h"
 
 namespace wolf
 {
diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp
index 360c9c5e8..f1705db6e 100644
--- a/src/association/association_node.cpp
+++ b/src/association/association_node.cpp
@@ -1,5 +1,5 @@
 
-#include "association_node.h"
+#include "base/association/association_node.h"
 
 AssociationNode::AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root) :
     is_root_(_is_root),
diff --git a/src/association/association_solver.cpp b/src/association/association_solver.cpp
index 0e31e4cfc..f640efa3e 100644
--- a/src/association/association_solver.cpp
+++ b/src/association/association_solver.cpp
@@ -1,5 +1,5 @@
 
-#include "association_solver.h"
+#include "base/association/association_solver.h"
 
 namespace wolf
 {
diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp
index e42f51b5d..1337e4bab 100644
--- a/src/association/association_tree.cpp
+++ b/src/association/association_tree.cpp
@@ -1,5 +1,5 @@
 
-#include "association_tree.h"
+#include "base/association/association_tree.h"
 
 namespace wolf
 {
diff --git a/src/capture/capture_GPS.cpp b/src/capture/capture_GPS.cpp
index 8c27eebd1..5c6520810 100644
--- a/src/capture/capture_GPS.cpp
+++ b/src/capture/capture_GPS.cpp
@@ -1,4 +1,4 @@
-#include "capture_GPS.h"
+#include "base/capture/capture_GPS.h"
 
 namespace wolf {
 
@@ -11,7 +11,6 @@ CaptureGPS::CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsut
 //    std::cout << obs_.toString() << std::endl;
 }
 
-
 CaptureGPS::~CaptureGPS()
 {
     //std::cout << "deleting CaptureGPS " << id() << std::endl;
@@ -22,5 +21,4 @@ rawgpsutils::SatellitesObs &CaptureGPS::getData()
     return obs_;
 }
 
-
 } //namespace wolf
diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp
index f31273687..052bf8f63 100644
--- a/src/capture/capture_GPS_fix.cpp
+++ b/src/capture/capture_GPS_fix.cpp
@@ -1,5 +1,4 @@
-#include "capture_GPS_fix.h"
-
+#include "base/capture/capture_GPS_fix.h"
 
 namespace wolf {
 
@@ -32,5 +31,4 @@ void CaptureGPSFix::process()
     getFeatureList().front()->addConstraint(std::make_shared <ConstraintGPS2D>(getFeatureList().front()));
 }
 
-
 } //namespace wolf
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
index 3f5195dad..6dbc07d59 100644
--- a/src/capture/capture_IMU.cpp
+++ b/src/capture/capture_IMU.cpp
@@ -1,11 +1,9 @@
-#include "capture_IMU.h"
-#include "sensor_IMU.h"
-#include "state_quaternion.h"
+#include "base/capture/capture_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/state_quaternion.h"
 
 namespace wolf {
 
-
-
 CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
                        SensorBasePtr _sensor_ptr,
                        const Eigen::Vector6s& _acc_gyro_data,
@@ -27,7 +25,6 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
     //
 }
 
-
 CaptureIMU::~CaptureIMU()
 {
     //
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 880303f49..051b93c9f 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -1,5 +1,5 @@
-#include "capture_base.h"
-#include "sensor_base.h"
+#include "base/capture/capture_base.h"
+#include "base/sensor/sensor_base.h"
 
 namespace wolf{
 
@@ -59,7 +59,6 @@ CaptureBase::CaptureBase(const std::string& _type,
 //    WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
 }
 
-
 CaptureBase::~CaptureBase()
 {
     removeStateBlocks();
@@ -294,8 +293,5 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
     }
 }
 
-
-
-
 } // namespace wolf
 
diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp
index 6a7950d62..1d8ac8b8d 100644
--- a/src/capture/capture_image.cpp
+++ b/src/capture/capture_image.cpp
@@ -1,4 +1,4 @@
-#include "capture_image.h"
+#include "base/capture/capture_image.h"
 
 namespace wolf {
 
@@ -45,5 +45,4 @@ std::vector<cv::KeyPoint>& CaptureImage::getKeypoints()
     return frame_.getKeyPoints();
 }
 
-
 } // namespace wolf
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
index 8d1a4ffea..0c3c6fe58 100644
--- a/src/capture/capture_laser_2D.cpp
+++ b/src/capture/capture_laser_2D.cpp
@@ -1,4 +1,4 @@
-#include "capture_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
 
 namespace wolf {
 
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 07fcd6c68..439228aa9 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "capture_motion.h"
+#include "base/capture/capture_motion.h"
 
 namespace wolf
 {
@@ -46,15 +46,11 @@ CaptureMotion::CaptureMotion(const std::string & _type,
     //
 }
 
-
-
-
 CaptureMotion::~CaptureMotion()
 {
     //
 }
 
-
 Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
 {
     VectorXs calib_preint    = getCalibrationPreint();
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
index 5d8eabccb..99b6e4281 100644
--- a/src/capture/capture_odom_2D.cpp
+++ b/src/capture/capture_odom_2D.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "capture_odom_2D.h"
+#include "base/capture/capture_odom_2D.h"
 
 namespace wolf
 {
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
index f6ba1dea9..26f4e6d6f 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "capture_odom_3D.h"
+#include "base/capture/capture_odom_3D.h"
 
 namespace wolf
 {
@@ -44,5 +44,3 @@ Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const Vector
 
 } /* namespace wolf */
 
-
-
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index c8422aa32..96aab020b 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -1,4 +1,4 @@
-#include "capture_pose.h"
+#include "base/capture/capture_pose.h"
 
 namespace wolf{
 
@@ -30,5 +30,4 @@ void CapturePose::emplaceFeatureAndConstraint()
         throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
 }
 
-
 } // namespace wolf
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
index 07062242a..9c4e7675f 100644
--- a/src/capture/capture_velocity.cpp
+++ b/src/capture/capture_velocity.cpp
@@ -1,4 +1,4 @@
-#include "capture_velocity.h"
+#include "base/capture/capture_velocity.h"
 
 namespace wolf {
 
diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp
index 721353d2f..44bf87788 100644
--- a/src/capture/capture_void.cpp
+++ b/src/capture/capture_void.cpp
@@ -1,4 +1,4 @@
-#include "capture_void.h"
+#include "base/capture/capture_void.h"
 
 namespace wolf {
 
@@ -13,5 +13,4 @@ CaptureVoid::~CaptureVoid()
 	//std::cout << "deleting CaptureVoid " << id() << std::endl;
 }
 
-
 } // namespace wolf
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
index a43786df6..9290da89a 100644
--- a/src/capture/capture_wheel_joint_position.cpp
+++ b/src/capture/capture_wheel_joint_position.cpp
@@ -1,6 +1,6 @@
-#include "capture_wheel_joint_position.h"
-#include "../sensors/sensor_diff_drive.h"
-#include "../rotations.h"
+#include "base/capture/capture_wheel_joint_position.h"
+#include "base/sensor/sensor_diff_drive.h"
+#include "base/rotations.h"
 
 namespace wolf {
 
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 26babf9af..e7e145567 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,9 +1,9 @@
-#include "ceres_manager.h"
-#include "create_numeric_diff_cost_function.h"
-#include "../trajectory_base.h"
-#include "../map_base.h"
-#include "../landmark_base.h"
-#include "../make_unique.h"
+#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/landmark/landmark_base.h"
+#include "base/make_unique.h"
 
 namespace wolf {
 
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
index 7b7ae9414..5cade1553 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp
@@ -1,4 +1,4 @@
-#include "local_parametrization_wrapper.h"
+#include "base/ceres_wrapper/local_parametrization_wrapper.h"
 
 namespace wolf {
 
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 36caa290b..9c63afd03 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -154,7 +154,6 @@ void QRManager::addConstraint(ConstraintBasePtr _ctr_ptr)
     pending_changes_ = true;
 }
 
-
 void QRManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
 {
     //std::cout << "remove constraint " << _ctr_ptr->id() << std::endl;
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index 2a5d8efec..cdaae51c3 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -1,7 +1,7 @@
-#include "solver_manager.h"
-#include "../trajectory_base.h"
-#include "../map_base.h"
-#include "../landmark_base.h"
+#include "base/solver/solver_manager.h"
+#include "base/trajectory_base.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
 
 namespace wolf {
 
diff --git a/src/constraint/constraint_analytic.cpp b/src/constraint/constraint_analytic.cpp
index 8b0422380..23ab82362 100644
--- a/src/constraint/constraint_analytic.cpp
+++ b/src/constraint/constraint_analytic.cpp
@@ -1,5 +1,5 @@
-#include "constraint_analytic.h"
-#include "state_block.h"
+#include "base/constraint/constraint_analytic.h"
+#include "base/state_block.h"
 
 namespace wolf {
 
diff --git a/src/constraint/constraint_base.cpp b/src/constraint/constraint_base.cpp
index 3e33cc6cc..f521cf214 100644
--- a/src/constraint/constraint_base.cpp
+++ b/src/constraint/constraint_base.cpp
@@ -1,6 +1,6 @@
-#include "constraint_base.h"
-#include "frame_base.h"
-#include "landmark_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/frame_base.h"
+#include "base/landmark/landmark_base.h"
 
 namespace wolf {
 
diff --git a/examples/.gitignore b/src/examples/.gitignore
similarity index 100%
rename from examples/.gitignore
rename to src/examples/.gitignore
diff --git a/examples/ACTIVESEARCH.yaml b/src/examples/ACTIVESEARCH.yaml
similarity index 100%
rename from examples/ACTIVESEARCH.yaml
rename to src/examples/ACTIVESEARCH.yaml
diff --git a/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
similarity index 100%
rename from examples/CMakeLists.txt
rename to src/examples/CMakeLists.txt
diff --git a/examples/Test_ORB.png b/src/examples/Test_ORB.png
similarity index 100%
rename from examples/Test_ORB.png
rename to src/examples/Test_ORB.png
diff --git a/examples/camera_params.yaml b/src/examples/camera_params.yaml
similarity index 100%
rename from examples/camera_params.yaml
rename to src/examples/camera_params.yaml
diff --git a/examples/camera_params_canonical.yaml b/src/examples/camera_params_canonical.yaml
similarity index 100%
rename from examples/camera_params_canonical.yaml
rename to src/examples/camera_params_canonical.yaml
diff --git a/examples/camera_params_ueye.yaml b/src/examples/camera_params_ueye.yaml
similarity index 100%
rename from examples/camera_params_ueye.yaml
rename to src/examples/camera_params_ueye.yaml
diff --git a/examples/camera_params_ueye_radial_dist.yaml b/src/examples/camera_params_ueye_radial_dist.yaml
similarity index 100%
rename from examples/camera_params_ueye_radial_dist.yaml
rename to src/examples/camera_params_ueye_radial_dist.yaml
diff --git a/examples/camera_params_ueye_sim.yaml b/src/examples/camera_params_ueye_sim.yaml
similarity index 100%
rename from examples/camera_params_ueye_sim.yaml
rename to src/examples/camera_params_ueye_sim.yaml
diff --git a/examples/input_M3500b_toro.graph b/src/examples/input_M3500b_toro.graph
similarity index 100%
rename from examples/input_M3500b_toro.graph
rename to src/examples/input_M3500b_toro.graph
diff --git a/examples/map_polyline_example.yaml b/src/examples/map_polyline_example.yaml
similarity index 100%
rename from examples/map_polyline_example.yaml
rename to src/examples/map_polyline_example.yaml
diff --git a/examples/processor_image_feature.yaml b/src/examples/processor_image_feature.yaml
similarity index 100%
rename from examples/processor_image_feature.yaml
rename to src/examples/processor_image_feature.yaml
diff --git a/examples/processor_image_vision_utils.yaml b/src/examples/processor_image_vision_utils.yaml
similarity index 100%
rename from examples/processor_image_vision_utils.yaml
rename to src/examples/processor_image_vision_utils.yaml
diff --git a/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
similarity index 100%
rename from examples/processor_imu.yaml
rename to src/examples/processor_imu.yaml
diff --git a/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
similarity index 100%
rename from examples/processor_imu_no_vote.yaml
rename to src/examples/processor_imu_no_vote.yaml
diff --git a/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
similarity index 100%
rename from examples/processor_imu_t1.yaml
rename to src/examples/processor_imu_t1.yaml
diff --git a/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
similarity index 100%
rename from examples/processor_imu_t6.yaml
rename to src/examples/processor_imu_t6.yaml
diff --git a/examples/processor_odom_3D.yaml b/src/examples/processor_odom_3D.yaml
similarity index 100%
rename from examples/processor_odom_3D.yaml
rename to src/examples/processor_odom_3D.yaml
diff --git a/examples/processor_tracker_feature_trifocal.yaml b/src/examples/processor_tracker_feature_trifocal.yaml
similarity index 100%
rename from examples/processor_tracker_feature_trifocal.yaml
rename to src/examples/processor_tracker_feature_trifocal.yaml
diff --git a/examples/sensor_imu.yaml b/src/examples/sensor_imu.yaml
similarity index 100%
rename from examples/sensor_imu.yaml
rename to src/examples/sensor_imu.yaml
diff --git a/examples/sensor_odom_3D.yaml b/src/examples/sensor_odom_3D.yaml
similarity index 100%
rename from examples/sensor_odom_3D.yaml
rename to src/examples/sensor_odom_3D.yaml
diff --git a/examples/sensor_odom_3D_HQ.yaml b/src/examples/sensor_odom_3D_HQ.yaml
similarity index 100%
rename from examples/sensor_odom_3D_HQ.yaml
rename to src/examples/sensor_odom_3D_HQ.yaml
diff --git a/examples/solver/test_SPQR.cpp b/src/examples/solver/test_SPQR.cpp
similarity index 99%
rename from examples/solver/test_SPQR.cpp
rename to src/examples/solver/test_SPQR.cpp
index bbd535de6..04592dbd0 100644
--- a/examples/solver/test_SPQR.cpp
+++ b/src/examples/solver/test_SPQR.cpp
@@ -45,7 +45,6 @@ int main (int argc, char **argv)
     std::cout << "solved x_" << std::endl << x_ << std::endl;
     std::cout << "ordering: " << solver.colsPermutation().indices().transpose() << std::endl;
 
-
     ///////////////////////////////////////////////////////////////////////
     // Directly in suitesparse
     cholmod_common Common, *cc ;
diff --git a/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
similarity index 99%
rename from examples/solver/test_ccolamd.cpp
rename to src/examples/solver/test_ccolamd.cpp
index 41c742ad6..ebba25db7 100644
--- a/examples/solver/test_ccolamd.cpp
+++ b/src/examples/solver/test_ccolamd.cpp
@@ -6,7 +6,7 @@
  */
 
 // Wolf includes
-#include "wolf.h"
+#include "base/wolf.h"
 
 //std includes
 #include <cstdlib>
diff --git a/examples/solver/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp
similarity index 99%
rename from examples/solver/test_ccolamd_blocks.cpp
rename to src/examples/solver/test_ccolamd_blocks.cpp
index 8c5c91219..b3d4d646b 100644
--- a/examples/solver/test_ccolamd_blocks.cpp
+++ b/src/examples/solver/test_ccolamd_blocks.cpp
@@ -5,7 +5,6 @@
  *      Author: jvallve
  */
 
-
 //std includes
 #include <cstdlib>
 #include <iostream>
@@ -52,7 +51,6 @@ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, i
     perm_blocks.indices() = idx_blocks;
 }
 
-
 //main
 int main(int argc, char *argv[])
 {
@@ -180,6 +178,3 @@ int main(int argc, char *argv[])
     //std::cout << "x = " << x_block_ordered.transpose() << std::endl;
 }
 
-
-
-
diff --git a/examples/solver/test_iQR.cpp b/src/examples/solver/test_iQR.cpp
similarity index 99%
rename from examples/solver/test_iQR.cpp
rename to src/examples/solver/test_iQR.cpp
index 79bfb03b3..868e84440 100644
--- a/examples/solver/test_iQR.cpp
+++ b/src/examples/solver/test_iQR.cpp
@@ -12,7 +12,6 @@
  *      Author: jvallve
  */
 
-
 //std includes
 #include <cstdlib>
 #include <iostream>
@@ -97,7 +96,6 @@ void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const i
     perm.indices() = new_indices;
 }
 
-
 //main
 int main(int argc, char *argv[])
 {
@@ -350,8 +348,3 @@ int main(int argc, char *argv[])
     }
 }
 
-
-
-
-
-
diff --git a/examples/solver/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp
similarity index 99%
rename from examples/solver/test_iQR_wolf.cpp
rename to src/examples/solver/test_iQR_wolf.cpp
index dd8707b8b..70624a0d6 100644
--- a/examples/solver/test_iQR_wolf.cpp
+++ b/src/examples/solver/test_iQR_wolf.cpp
@@ -5,7 +5,6 @@
  *      Author: jvallve
  */
 
-
 //std includes
 #include <cstdlib>
 #include <string>
@@ -222,7 +221,6 @@ class SolverQR
 
                 A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
 
-
                 assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
                 assert(_meas.jacobians.at(j).rows() == _meas.dim);
 
@@ -396,7 +394,6 @@ class SolverQR
             return 1;
         }
 
-
         void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
         {
             ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
@@ -561,8 +558,3 @@ int main(int argc, char *argv[])
     }
 }
 
-
-
-
-
-
diff --git a/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
similarity index 99%
rename from examples/solver/test_iQR_wolf2.cpp
rename to src/examples/solver/test_iQR_wolf2.cpp
index 07aba80d4..d3fb71cad 100644
--- a/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -17,9 +17,9 @@
 #include <queue>
 
 //Wolf includes
-#include "state_block.h"
-#include "constraint_base.h"
-#include "sensor_laser_2D.h"
+#include "base/state_block.h"
+#include "base/constraint/constraint_base.h"
+#include "base/sensor/sensor_laser_2D.h"
 #include "wolf_manager.h"
 
 // wolf solver
@@ -35,14 +35,12 @@
 
 //Ceres includes
 #include "glog/logging.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 //laser_scan_utils
 #include "iri-algorithms/laser_scan_utils/corner_detector.h"
 #include "iri-algorithms/laser_scan_utils/entities.h"
 
-
-
 //function travel around
 void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
 {
@@ -96,7 +94,6 @@ void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double
     pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
 }
 
-
 //main
 int main(int argc, char *argv[])
 {
@@ -118,7 +115,6 @@ int main(int argc, char *argv[])
 
     // INITIALIZATION ============================================================================================
 
-
     //init random generators
     Scalar odom_std_factor = 0.1;
     Scalar gps_std = 10;
@@ -432,8 +428,3 @@ int main(int argc, char *argv[])
     return 0;
 }
 
-
-
-
-
-
diff --git a/examples/solver/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
similarity index 99%
rename from examples/solver/test_incremental_ccolamd_blocks.cpp
rename to src/examples/solver/test_incremental_ccolamd_blocks.cpp
index de4f82f00..792250a81 100644
--- a/examples/solver/test_incremental_ccolamd_blocks.cpp
+++ b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
@@ -5,7 +5,6 @@
  *      Author: jvallve
  */
 
-
 //std includes
 #include <cstdlib>
 #include <iostream>
@@ -54,7 +53,6 @@ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, i
     perm_blocks.indices() = idx_blocks;
 }
 
-
 //main
 int main(int argc, char *argv[])
 {
@@ -143,7 +141,6 @@ int main(int argc, char *argv[])
         // r.h.v
         b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1);
 
-
         std::cout << "Solving factor graph:" << std::endl;
         std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl;
 //        std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl;
@@ -160,7 +157,6 @@ int main(int argc, char *argv[])
         x = solver.solve(b);
         time1 += ((double) clock() - t1) / CLOCKS_PER_SEC;
 
-
         // SOLVING WITH REORDERING ------------------------------------
         // Order with previous orderings
         acc_permutation.conservativeResize(dim*(i+1));
@@ -198,7 +194,6 @@ int main(int argc, char *argv[])
         x_ordered = acc_permutation_matrix.inverse() * x_ordered;
         time2 += ((double) clock() - t2) / CLOCKS_PER_SEC;
 
-
         // SOLVING WITH BLOCK REORDERING ------------------------------------
         // Order with previous orderings
         acc_permutation_b.conservativeResize(dim*(i+1));
@@ -245,7 +240,6 @@ int main(int argc, char *argv[])
         x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered;
         time3 += ((double) clock() - t3) / CLOCKS_PER_SEC;
 
-
         // RESULTS ------------------------------------
         std::cout << "========================= RESULTS " << i << ":" << std::endl;
         std::cout << "NO REORDERING:    solved in " << time1*1e3 << " ms" << std::endl;
@@ -266,6 +260,3 @@ int main(int argc, char *argv[])
         //std::cout << "x = " << x_b_ordered.transpose() << std::endl;
 }
 
-
-
-
diff --git a/examples/solver/test_permutations.cpp b/src/examples/solver/test_permutations.cpp
similarity index 99%
rename from examples/solver/test_permutations.cpp
rename to src/examples/solver/test_permutations.cpp
index 5d28ec06c..c33c744c6 100644
--- a/examples/solver/test_permutations.cpp
+++ b/src/examples/solver/test_permutations.cpp
@@ -5,7 +5,6 @@
  *      Author: jvallve
  */
 
-
 //std includes
 #include <cstdlib>
 #include <iostream>
@@ -19,7 +18,6 @@
 // eigen includes
 #include <eigen3/Eigen/OrderingMethods>
 
-
 using namespace Eigen;
 
 //main
@@ -111,6 +109,5 @@ int main(int argc, char *argv[])
     std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl;
     std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl;
 
-
 //    Map<>
 }
diff --git a/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
similarity index 97%
rename from examples/test_2_lasers_offline.cpp
rename to src/examples/test_2_lasers_offline.cpp
index 5c5672c79..d8d91b638 100644
--- a/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -1,5 +1,5 @@
 //std includes
-#include <sensor_GPS_fix.h>
+#include "base/sensor/sensor_GPS_fix.h"
 #include <cstdlib>
 #include <iostream>
 #include <fstream>
@@ -17,12 +17,12 @@
 #include "glog/logging.h"
 
 //Wolf includes
-#include "../problem.h"
-#include "../processor_tracker_landmark_corner.h"
-#include "../processor_odom_2D.h"
-#include "../sensor_laser_2D.h"
-#include "../sensor_odom_2D.h"
-#include "../ceres_wrapper/ceres_manager.h"
+#include "base/problem.h"
+#include "base/processor/processor_tracker_landmark_corner.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // laserscanutils
 #include "laser_scan_utils/line_finder_iterative.h"
@@ -31,7 +31,7 @@
 //C includes for sleep, time and main args
 #include "unistd.h"
 
-#include "../capture_pose.h"
+#include "base/capture/capture_pose.h"
 //faramotics includes
 #include "faramotics/dynamicSceneRender.h"
 #include "faramotics/rangeScan2D.h"
@@ -88,7 +88,6 @@ int main(int argc, char** argv)
     else
         std::cout << "Simulated data files opened correctly..." << std::endl;
 
-
     unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
 
     // INITIALIZATION ============================================================================================
@@ -226,7 +225,6 @@ int main(int argc, char** argv)
         }
         mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
 
-
         // SOLVE OPTIMIZATION ---------------------------
         std::cout << "SOLVING..." << std::endl;
         t1 = clock();
diff --git a/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp
similarity index 99%
rename from examples/test_analytic_odom_constraint.cpp
rename to src/examples/test_analytic_odom_constraint.cpp
index 029b0280b..854d4543f 100644
--- a/examples/test_analytic_odom_constraint.cpp
+++ b/src/examples/test_analytic_odom_constraint.cpp
@@ -12,8 +12,8 @@
 
 //Wolf includes
 #include "wolf_manager.h"
-#include "capture_void.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
@@ -28,8 +28,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr
 
   original.makeCompressed();
 }
-}
-
 
 int main(int argc, char** argv) 
 {
@@ -71,8 +69,6 @@ int main(int argc, char** argv)
     CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options);
     CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options);
 
-
-
     // load graph from .txt
     offLineFile_.open(file_path_.c_str(), std::ifstream::in);
     if (offLineFile_.is_open())
diff --git a/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
similarity index 99%
rename from examples/test_autodiff.cpp
rename to src/examples/test_autodiff.cpp
index c027f9fcc..a60563546 100644
--- a/examples/test_autodiff.cpp
+++ b/src/examples/test_autodiff.cpp
@@ -17,8 +17,8 @@
 
 //Wolf includes
 #include "wolf_manager.h"
-#include "sensor_laser_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 //C includes for sleep, time and main args
 #include "unistd.h"
@@ -49,7 +49,6 @@ void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double
     pose.moveForward(displacement_);
     pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
 }
-}
 
 int main(int argc, char** argv)
 {
diff --git a/examples/test_capture_laser_2D.cpp b/src/examples/test_capture_laser_2D.cpp
similarity index 99%
rename from examples/test_capture_laser_2D.cpp
rename to src/examples/test_capture_laser_2D.cpp
index f5c43ac41..8ca0e376b 100644
--- a/examples/test_capture_laser_2D.cpp
+++ b/src/examples/test_capture_laser_2D.cpp
@@ -3,7 +3,7 @@
 #include <random>
 
 //wolf
-#include "capture_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
 
 // Eigen in std vector
 #include <Eigen/StdVector>
diff --git a/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
similarity index 98%
rename from examples/test_ceres_2_lasers.cpp
rename to src/examples/test_ceres_2_lasers.cpp
index 679777dd4..897838353 100644
--- a/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -1,5 +1,5 @@
 //std includes
-#include <sensor_GPS_fix.h>
+#include "base/sensor/sensor_GPS_fix.h"
 #include <cstdlib>
 #include <iostream>
 #include <fstream>
@@ -17,12 +17,12 @@
 #include "glog/logging.h"
 
 //Wolf includes
-#include "problem.h"
-#include "processor_tracker_landmark_corner.h"
-#include "processor_odom_2D.h"
-#include "sensor_laser_2D.h"
-#include "sensor_odom_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/problem.h"
+#include "base/processor/processor_tracker_landmark_corner.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // laserscanutils
 #include "laser_scan_utils/line_finder_iterative.h"
@@ -31,7 +31,7 @@
 //C includes for sleep, time and main args
 #include "unistd.h"
 
-#include "../capture_pose.h"
+#include "base/capture/capture_pose.h"
 //faramotics includes
 #include "faramotics/dynamicSceneRender.h"
 #include "faramotics/rangeScan2D.h"
@@ -71,7 +71,6 @@ class FaramoticsRobot
             myScanner->loadAssimpModel(modelFileName);
         }
 
-
         //function travel around
         Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
         {
diff --git a/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
similarity index 98%
rename from examples/test_ceres_2_lasers_polylines.cpp
rename to src/examples/test_ceres_2_lasers_polylines.cpp
index 9668e1c4a..70928589a 100644
--- a/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -1,5 +1,5 @@
 //std includes
-#include <sensor_GPS_fix.h>
+#include "base/sensor/sensor_GPS_fix.h"
 #include <cstdlib>
 #include <iostream>
 #include <fstream>
@@ -17,12 +17,12 @@
 #include "glog/logging.h"
 
 //Wolf includes
-#include "problem.h"
-#include "processor_tracker_landmark_polyline.h"
-#include "processor_odom_2D.h"
-#include "sensor_laser_2D.h"
-#include "sensor_odom_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/problem.h"
+#include "base/processor/processor_tracker_landmark_polyline.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // laserscanutils
 #include "laser_scan_utils/line_finder_iterative.h"
@@ -31,7 +31,7 @@
 //C includes for sleep, time and main args
 #include "unistd.h"
 
-#include "../capture_pose.h"
+#include "base/capture/capture_pose.h"
 //faramotics includes
 #include "faramotics/dynamicSceneRender.h"
 #include "faramotics/rangeScan2D.h"
@@ -71,7 +71,6 @@ class FaramoticsRobot
             myScanner->loadAssimpModel(modelFileName);
         }
 
-
         //function travel around
         Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
         {
diff --git a/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
similarity index 96%
rename from examples/test_constraint_AHP.cpp
rename to src/examples/test_constraint_AHP.cpp
index 8e555cac0..7dc1eabe7 100644
--- a/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -1,10 +1,10 @@
-#include "pinhole_tools.h"
-#include "landmark_AHP.h"
-#include "constraint_AHP.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "sensor_camera.h"
-#include "capture_image.h"
+#include "base/pinhole_tools.h"
+#include "base/landmark/landmark_AHP.h"
+#include "base/constraint/constraint_AHP.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/capture/capture_image.h"
 
 int main()
 {
@@ -45,7 +45,6 @@ int main()
     // one-liner API
     ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml");
 
-
     // create the current frame
     Eigen::Vector7s frame_pos_ori;
     frame_pos_ori.setRandom();
@@ -71,7 +70,6 @@ int main()
     image_ptr = std::make_shared< CaptureImage>(t, camera_ptr_, frame);
     last_frame->addCapture(image_ptr);
 
-
     // create the feature
     cv::KeyPoint kp; kp.pt = {10,20};
     cv::Mat desc;
@@ -82,14 +80,12 @@ int main()
     FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
     //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectoryPtr()->getLastFramePtr();
 
-
     // create the landmark
     Eigen::Vector2s point2D;
     point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
     point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
     std::cout << "point2D: " << point2D.transpose() << std::endl;
 
-
     Scalar distance = 2; // arbitrary value
     Eigen::Vector4s vec_homogeneous;
 
@@ -115,8 +111,6 @@ int main()
 
     std::cout << "Landmark AHP created" << std::endl;
 
-
-
     // Create the constraint
     ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr);
 
@@ -146,7 +140,6 @@ int main()
             landmark_.data(), expectation.data());
 //    current_frame p; current_frame o; anchor_frame p; anchor_frame o; homogeneous_vector landmark, residual
 
-
     std::cout << "expectation computed" << std::endl;
     std::cout << "expectation = " << expectation[0] << "   " << expectation[1] << std::endl;
 
diff --git a/examples/test_constraint_imu.cpp b/src/examples/test_constraint_imu.cpp
similarity index 97%
rename from examples/test_constraint_imu.cpp
rename to src/examples/test_constraint_imu.cpp
index e4a3b8f2b..77464a9e4 100644
--- a/examples/test_constraint_imu.cpp
+++ b/src/examples/test_constraint_imu.cpp
@@ -1,14 +1,14 @@
 //Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "../capture_pose.h"
-#include "wolf.h"
-#include "problem.h"
-#include "constraint_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_IMU.h"
+#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/constraint/constraint_odom_3D.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 //#define DEBUG_RESULTS
 
@@ -56,7 +56,6 @@ int main(int argc, char** argv)
     CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
     imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back());
 
-
     // set variables
     using namespace std;
     Eigen::VectorXs state_vec;
@@ -237,7 +236,6 @@ int main(int argc, char** argv)
         wolf_problem_ptr_->print(4,1,1,1);
     }
 
-
     ///having a look at covariances
     Eigen::MatrixXs predelta_cov;
     predelta_cov.resize(9,9);
diff --git a/examples/test_constraint_odom_3D.cpp b/src/examples/test_constraint_odom_3D.cpp
similarity index 73%
rename from examples/test_constraint_odom_3D.cpp
rename to src/examples/test_constraint_odom_3D.cpp
index 41cf14f2d..5cdbfccf5 100644
--- a/examples/test_constraint_odom_3D.cpp
+++ b/src/examples/test_constraint_odom_3D.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "../src/constraint_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
 
 namespace wolf
 {
diff --git a/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
similarity index 97%
rename from examples/test_diff_drive.cpp
rename to src/examples/test_diff_drive.cpp
index 821f5bdd8..a36a92692 100644
--- a/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -6,11 +6,11 @@
  */
 
 //Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "../sensors/sensor_diff_drive.h"
-#include "../captures/capture_wheel_joint_position.h"
-#include "../processors/processor_diff_drive.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_diff_drive.h"
+#include "base/capture/capture_wheel_joint_position.h"
+#include "processor/processor_diff_drive.h"
 
 //std
 #include <iostream>
diff --git a/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
similarity index 97%
rename from examples/test_eigen_quaternion.cpp
rename to src/examples/test_eigen_quaternion.cpp
index cea232487..1c01f5e13 100644
--- a/examples/test_eigen_quaternion.cpp
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -6,7 +6,7 @@
 #include <eigen3/Eigen/Geometry>
 
 //Wolf
-#include "wolf.h"
+#include "base/wolf.h"
 
 int main()
 {
diff --git a/examples/test_eigen_template.cpp b/src/examples/test_eigen_template.cpp
similarity index 99%
rename from examples/test_eigen_template.cpp
rename to src/examples/test_eigen_template.cpp
index 3ea96954d..47e5aa419 100644
--- a/examples/test_eigen_template.cpp
+++ b/src/examples/test_eigen_template.cpp
@@ -5,7 +5,6 @@
  *      \author: jsola
  */
 
-
 #include <eigen3/Eigen/Dense>
 #include <eigen3/Eigen/Geometry>
 #include <iostream>
@@ -57,12 +56,10 @@ inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<D
     }
 }
 
-
 int main(void)
 {
     using namespace Eigen;
 
-
     VectorXd x(10);
     x << 1,2,3,4,5,6,7,8,9,10;
 
diff --git a/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
similarity index 98%
rename from examples/test_faramotics_simulation.cpp
rename to src/examples/test_faramotics_simulation.cpp
index 63f0cd8e7..1a3feedaf 100644
--- a/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -16,10 +16,10 @@
 #include "unistd.h"
 
 // wolf
-#include "wolf.h"
-#include "feature_base.h"
-#include "landmark_base.h"
-#include "state_block.h"
+#include "base/wolf.h"
+#include "base/feature/feature_base.h"
+#include "base/landmark/landmark_base.h"
+#include "base/state_block.h"
 
 //faramotics includes
 #include "faramotics/dynamicSceneRender.h"
@@ -60,7 +60,6 @@ class FaramoticsRobot
             myScanner->loadAssimpModel(modelFileName);
         }
 
-
         //function travel around
         Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
         {
@@ -216,7 +215,6 @@ int main(int argc, char** argv)
         // ROBOT MOVEMENT ---------------------------
         ground_truth = robot.motionCampus(step, odom_data(0), odom_data(1));
 
-
         // LIDAR DATA ---------------------------
         scan1 = robot.computeScan(1);
         scan2 = robot.computeScan(2);
diff --git a/examples/test_fcn_ptr.cpp b/src/examples/test_fcn_ptr.cpp
similarity index 99%
rename from examples/test_fcn_ptr.cpp
rename to src/examples/test_fcn_ptr.cpp
index 70b38393f..8ca82790e 100644
--- a/examples/test_fcn_ptr.cpp
+++ b/src/examples/test_fcn_ptr.cpp
@@ -15,7 +15,6 @@ double divide   (double _n, double _d)              { return _n/_d;     }
 double mult     (double _x, double _y)              { return _x*_y;     }
 double mult_div (double _x, double _y, double _z)   { return _x*_y/_z;  }
 
-
 //======== test_simple usage of function pointers ============
 typedef double (*FcnType)(double);
 
@@ -119,7 +118,6 @@ void test_var()
     std::cout << "4*3/6 = " << run_v((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl;
 }
 
-
 //####################################################################################
 
 int main()
diff --git a/examples/test_image.cpp b/src/examples/test_image.cpp
similarity index 97%
rename from examples/test_image.cpp
rename to src/examples/test_image.cpp
index c5afcc61b..feb794e1f 100644
--- a/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -1,10 +1,10 @@
 // Testing things for the 3D image odometry
 
 //Wolf includes
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "processor_tracker_feature_image.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/capture/capture_image.h"
+#include "base/processor/processor_tracker_feature_image.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // Vision utils includes
 #include <vision_utils.h>
diff --git a/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
similarity index 97%
rename from examples/test_imuDock.cpp
rename to src/examples/test_imuDock.cpp
index d68e8dd43..bcaa85d9c 100644
--- a/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -5,21 +5,21 @@
  *      \author: Dinesh Atchuthan
  */
 
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "ceres_wrapper/ceres_manager.h"
-#include "sensor_odom_3D.h"
-#include "processor_odom_3D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
 
 //Constraints headers
-#include "constraint_fix_bias.h"
+#include "base/constraint/constraint_fix_bias.h"
 
 //std
 #include <iostream>
 #include <fstream>
-#include "../constraint_pose_3D.h"
+#include "base/constraint/constraint_pose_3D.h"
 
 #define OUTPUT_RESULTS
 //#define ADD_KF3
diff --git a/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
similarity index 97%
rename from examples/test_imuDock_autoKFs.cpp
rename to src/examples/test_imuDock_autoKFs.cpp
index 03677027f..d0665c746 100644
--- a/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -5,21 +5,21 @@
  *      \author: Dinesh Atchuthan
  */
 
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "ceres_wrapper/ceres_manager.h"
-#include "sensor_odom_3D.h"
-#include "processor_odom_3D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
 
 //Constraints headers
-#include "constraint_fix_bias.h"
+#include "base/constraint/constraint_fix_bias.h"
 
 //std
 #include <iostream>
 #include <fstream>
-#include "../constraint_pose_3D.h"
+#include "base/constraint/constraint_pose_3D.h"
 
 #define OUTPUT_RESULTS
 //#define AUTO_KFS
diff --git a/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
similarity index 96%
rename from examples/test_imuPlateform_Offline.cpp
rename to src/examples/test_imuPlateform_Offline.cpp
index 9d834d73e..e1f9d23fb 100644
--- a/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -1,15 +1,15 @@
 //Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_odom_3D.h"
-#include "constraint_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#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/sensor/sensor_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 //std
 #include <iostream>
@@ -93,7 +93,6 @@ int main(int argc, char** argv)
     SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
     ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
 
-
     // SENSOR + PROCESSOR ODOM 3D
     SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
     ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
@@ -177,7 +176,6 @@ int main(int argc, char** argv)
     std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
     << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
 
-
     // Print statistics
     std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
     std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
@@ -235,7 +233,6 @@ int main(int argc, char** argv)
             for(int i = 0; i<16; i++)
                 cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
 
-
             debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
             << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
             << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
diff --git a/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
similarity index 97%
rename from examples/test_imu_constrained0.cpp
rename to src/examples/test_imu_constrained0.cpp
index 87330497c..d20f108d8 100644
--- a/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -1,15 +1,15 @@
 //Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_odom_3D.h"
-#include "constraint_odom_3D.h"
-#include "state_block.h"
-#include "state_quaternion.h"
-#include "processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#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/sensor/sensor_odom_3D.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 //std
 #include <iostream>
@@ -34,7 +34,6 @@ int main(int argc, char** argv)
     // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
     // FOR ODOM, file content is : Timestampt\t Δpx\t  Δpy\t  Δpz\t  Δox\t  Δoy\t  Δoz
     
-    
     std::ifstream imu_data_input;
     std::ifstream odom_data_input;
     const char * filename_imu;
@@ -115,7 +114,6 @@ int main(int argc, char** argv)
     SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
     ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
 
-
     // SENSOR + PROCESSOR ODOM 3D
     SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
     ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
@@ -195,7 +193,6 @@ int main(int argc, char** argv)
             t = ts;
             //std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
         
-        
             Eigen::VectorXs frm_state(16);
             frm_state = origin_KF->getState();
 
@@ -234,7 +231,6 @@ int main(int argc, char** argv)
     std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
     << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
 
-
     // Print statistics
     std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
     std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
@@ -300,7 +296,6 @@ int main(int argc, char** argv)
             for(int i = 0; i<16; i++)
                 cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
 
-
             debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
             << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
             << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
diff --git a/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
similarity index 93%
rename from examples/test_kf_callback.cpp
rename to src/examples/test_kf_callback.cpp
index 2822294e7..8f049c8d6 100644
--- a/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -5,12 +5,10 @@
  *      Author: jsola
  */
 
-
-
-#include "../sensor_odom_2D.h"
-#include "../processor_odom_2D.h"
-#include "../processor_tracker_feature_dummy.h"
-#include "../capture_void.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
 
 int main()
 {
diff --git a/examples/test_list_remove.cpp b/src/examples/test_list_remove.cpp
similarity index 99%
rename from examples/test_list_remove.cpp
rename to src/examples/test_list_remove.cpp
index 458ae19db..b0795d0a6 100644
--- a/examples/test_list_remove.cpp
+++ b/src/examples/test_list_remove.cpp
@@ -5,8 +5,6 @@
  *      Author: jsola
  */
 
-
-
 #include <memory>
 #include <list>
 #include <algorithm>
diff --git a/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
similarity index 94%
rename from examples/test_map_yaml.cpp
rename to src/examples/test_map_yaml.cpp
index 80273a384..7990919ca 100644
--- a/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -5,13 +5,13 @@
  *      \author: jsola
  */
 
-#include "wolf.h"
-#include "problem.h"
-#include "map_base.h"
-#include "landmark_polyline_2D.h"
-#include "landmark_AHP.h"
-#include "state_block.h"
-#include "../yaml/yaml_conversion.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/landmark/landmark_AHP.h"
+#include "base/state_block.h"
+#include "base/yaml/yaml_conversion.h"
 
 #include <iostream>
 using namespace wolf;
@@ -88,7 +88,6 @@ int main()
     std::string thisfile = __FILE__;
     problem->getMapPtr()->save(filename, "Example generated by test file " + thisfile);
 
-
     std::cout << "Clearing map... " << std::endl;
     problem->getMapPtr()->getLandmarkList().clear();
 
diff --git a/examples/test_matrix_prod.cpp b/src/examples/test_matrix_prod.cpp
similarity index 99%
rename from examples/test_matrix_prod.cpp
rename to src/examples/test_matrix_prod.cpp
index a99ff1424..b03068283 100644
--- a/examples/test_matrix_prod.cpp
+++ b/src/examples/test_matrix_prod.cpp
@@ -88,8 +88,6 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,1>)
         std::cout << "Time Co = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N \
         << "ns <-- this is the Eigen default!" << std::endl;
 
-
-
 /**
  * We multiply matrices and see how long it takes.
  * We compare different combinations of row-major and column-major to see which one is the fastest.
diff --git a/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
similarity index 97%
rename from examples/test_mpu.cpp
rename to src/examples/test_mpu.cpp
index 4acbf55b7..08db29a81 100644
--- a/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -6,11 +6,11 @@
  */
 
  //Wolf
-#include <capture_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#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 <iostream>
 #include <fstream>
 #include <iomanip>
@@ -18,8 +18,8 @@
 #include <cmath>
 #include <termios.h>
 #include <fcntl.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
 
 #define DEBUG_RESULTS
 #define FROM_FILE
@@ -194,7 +194,6 @@ int main(int argc, char** argv)
     std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
     << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
 
-
     // Print statistics
     std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
     std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
diff --git a/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
similarity index 97%
rename from examples/test_processor_imu.cpp
rename to src/examples/test_processor_imu.cpp
index 119e2fe62..f9b57a323 100644
--- a/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -6,13 +6,13 @@
  */
 
 //Wolf
-#include <capture_IMU.h>
-#include <processor_IMU.h>
-#include <sensor_IMU.h>
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#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 <iostream>
 #include <fstream>
 #include <iomanip>
@@ -21,7 +21,6 @@
 
 //#define DEBUG_RESULTS
 
-
 int main(int argc, char** argv)
 {
     using namespace wolf;
@@ -185,7 +184,6 @@ int main(int argc, char** argv)
 //    std::cout << "Integrated std  : " << std::fixed << std::setprecision(3) << std::setw(8)
 //    << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
 
-
     // Print statistics
     std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
     std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
diff --git a/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
similarity index 99%
rename from examples/test_processor_imu_jacobians.cpp
rename to src/examples/test_processor_imu_jacobians.cpp
index 6cdb01c97..e876a21d9 100644
--- a/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -6,13 +6,13 @@
  */
 
 //Wolf
-#include <capture_IMU.h>
-#include <sensor_IMU.h>
+#include "base/capture/capture_IMU.h"
+#include "base/sensor/sensor_IMU.h"
 #include <test/processor_IMU_UnitTester.h>
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 #include <iostream>
 #include <fstream>
 #include <iomanip>
@@ -192,7 +192,6 @@ int main(int argc, char** argv)
     else
         std::cout<< "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
 
-
     /*              Numerical method to check jacobians wrt noise
 
                                                             dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz]
diff --git a/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
similarity index 91%
rename from examples/test_processor_odom_3D.cpp
rename to src/examples/test_processor_odom_3D.cpp
index b7c2c9a9d..c9321b51e 100644
--- a/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -5,14 +5,13 @@
  *      \author: jsola
  */
 
-
-#include <capture_IMU.h>
-#include "problem.h"
-#include "sensor_odom_2D.h"
-#include "processor_odom_3D.h"
-#include "map_base.h"
-#include "landmark_base.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_IMU.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 #include <cstdlib>
 
@@ -25,8 +24,6 @@ using Eigen::Vector7s;
 using Eigen::Quaternions;
 using Eigen::VectorXs;
 
-
-
 int main (int argc, char** argv)
 {
     cout << "\n========= Test ProcessorOdom3D ===========" << endl;
diff --git a/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
similarity index 87%
rename from examples/test_processor_tracker_feature.cpp
rename to src/examples/test_processor_tracker_feature.cpp
index 943e18010..ab22be418 100644
--- a/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -9,12 +9,12 @@
 #include <iostream>
 
 //Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_base.h"
-#include "state_block.h"
-#include "processor_tracker_feature_dummy.h"
-#include "capture_void.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
 
 int main()
 {
diff --git a/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
similarity index 93%
rename from examples/test_processor_tracker_landmark.cpp
rename to src/examples/test_processor_tracker_landmark.cpp
index bbf3e4132..15ed24d72 100644
--- a/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -9,12 +9,12 @@
 #include <iostream>
 
 //Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "sensor_base.h"
-#include "state_block.h"
-#include "processor_tracker_landmark_dummy.h"
-#include "capture_void.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/processor/processor_tracker_landmark_dummy.h"
+#include "base/capture/capture_void.h"
 
 void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
 {
@@ -87,9 +87,6 @@ int main()
 
     wolf_problem_ptr_->print(2);
 
-
     return 0;
 }
 
-
-
diff --git a/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
similarity index 96%
rename from examples/test_processor_tracker_landmark_image.cpp
rename to src/examples/test_processor_tracker_landmark_image.cpp
index 5a1915901..f5b084e13 100644
--- a/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -1,18 +1,18 @@
 //std
 #include <iostream>
 
-#include "processor_tracker_landmark_image.h"
+#include "base/processor/processor_tracker_landmark_image.h"
 
 //Wolf
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "processor_odom_3D.h"
-#include "sensor_odom_3D.h"
-#include "sensor_camera.h"
-#include "capture_image.h"
-#include "capture_pose.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/state_block.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/capture/capture_image.h"
+#include "base/capture/capture_pose.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // Vision utils includes
 #include <vision_utils.h>
@@ -221,12 +221,10 @@ int main(int argc, char** argv)
 //        std::cout << "current   ts: " << t.get() << std::endl;
 //        std::cout << "          dt: " << t_prev - t_prev_prev << "; dx: " << dx.transpose() << std::endl;
 
-
         // Cleanup map ---------------------------------------
 
         cleanupMap(problem, t, 2, 5); // dt, min_ctr
 
-
         // Solve -----------------------------------------------
         // solve only when new KFs are added
         if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
@@ -236,7 +234,6 @@ int main(int argc, char** argv)
             std::cout << summary << std::endl;
         }
 
-
         // Finish loop -----------------------------------------
         cv::Mat image_graphics = frame_buff.back()->getImage().clone();
         prc_img_ptr->drawTrackerRoi(image_graphics, cv::Scalar(255.0, 0.0, 255.0)); //tracker roi
diff --git a/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp
similarity index 99%
rename from examples/test_projection_points.cpp
rename to src/examples/test_projection_points.cpp
index 7945c939f..c24e8904e 100644
--- a/examples/test_projection_points.cpp
+++ b/src/examples/test_projection_points.cpp
@@ -6,7 +6,7 @@
 #include <iostream>
 
 //wolf includes
-#include "pinhole_tools.h"
+#include "base/pinhole_tools.h"
 
 int main(int argc, char** argv)
 {
@@ -41,7 +41,6 @@ int main(int argc, char** argv)
     for (int ii = 0; ii < points3D.rows(); ++ii)
         std::cout << "points2D- X: " << points2D(ii,0) << "; Y: " << points2D(ii,1) << std::endl;
 
-
     std::cout << std::endl << " ========= PinholeTools DUALITY TEST ===========" << std::endl << std::endl;
 
     //================================= projectPoint and backprojectPoint to/from NormalizedPlane
@@ -67,14 +66,12 @@ int main(int argc, char** argv)
             pinhole::backprojectPointFromNormalizedPlane(project_point_normalized_output,
                                                          backproject_point_normalized_depth);
 
-
     std::cout << "TEST project and backproject PointToNormalizedPlane" << std::endl;
     std::cout << std:: endl << "Original         " << project_point_normalized_test.transpose() << std::endl;
     std::cout << std:: endl << "Project          " << project_point_normalized_output.transpose() << std::endl;
     std::cout << std:: endl << "Alternate project" << project_point_normalized_output.transpose() << std::endl;
     std::cout << std:: endl << "Backproject      " << backproject_point_normalized_output.transpose() << std::endl;
 
-
     //================================= projectPoint and backprojectPoint to/from NormalizedPlane WITH JACOBIANS
 
     Eigen::Vector3s pp_normalized_test;
@@ -92,7 +89,6 @@ int main(int argc, char** argv)
     Eigen::Matrix<Scalar, 3, 2> bpp_normalized_jacobian;
     Eigen::Vector3s bpp_normalized_jacobian_depth;
 
-
     pinhole::projectPointToNormalizedPlane(pp_normalized_test,
                                            pp_normalized_output,
                                            pp_normalized_jacobian);
@@ -123,13 +119,11 @@ int main(int argc, char** argv)
     std::cout << "\n--> Jacobian" << std::endl << bpp_normalized_jacobian << std::endl;
     std::cout << "\n--> Jacobian - depth" << bpp_normalized_jacobian_depth.transpose() << std::endl;
 
-
     Eigen::Matrix2s test_jacobian; // (2x3) * (3x2) = (2x2)
     test_jacobian =  pp_normalized_jacobian * bpp_normalized_jacobian;
 
     std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian << std::endl;
 
-
     //================================= IsInRoi / IsInImage
 
     Eigen::Vector2s pix;
@@ -166,8 +160,6 @@ int main(int argc, char** argv)
     std::cout << "width: " << image_width << "; height: " << image_height << std::endl;
     std::cout << "is_in_image: " << is_in_image << std::endl;
 
-
-
     //================================= computeCorrectionModel
 
     Eigen::Vector2s distortion2;
@@ -194,11 +186,8 @@ int main(int argc, char** argv)
     std::cout << std::endl << "correction" << std::endl;
     std::cout << "c1: " << correction_test2[0] << "; c2: " << correction_test2[1] << std::endl;
 
-
-
     //================================= distortPoint
 
-
     Eigen::Vector2s distorting_point;
     distorting_point[0] = 0.35355;
     distorting_point[1] = 0.35355;
@@ -237,8 +226,6 @@ int main(int argc, char** argv)
     std::cout << "\n--> Jacobian" << std::endl <<
                  distortion_jacobian2 << std::endl;
 
-
-
     Eigen::Vector2s corrected_point5;
     Eigen::Matrix2s corrected_jacobian2;
     pinhole::undistortPoint(correction_test2,
@@ -280,7 +267,6 @@ int main(int argc, char** argv)
     std::cout << std::endl << "Depixelized" << std::endl;
     std::cout << "x: " << depixelize_output3[0] << "; y: " << depixelize_output3[1] << std::endl;
 
-
     ////
 
     Eigen::Vector2s pixelize_output4;
@@ -298,7 +284,6 @@ int main(int argc, char** argv)
     std::cout << "\n--> Jacobian" << std::endl <<
                  pixelize_jacobian2 << std::endl;
 
-
     Eigen::Vector2s depixelize_output4;
     Eigen::Matrix2s depixelize_jacobian2;
     pinhole::depixellizePoint(k_test2,
@@ -317,12 +302,8 @@ int main(int argc, char** argv)
     std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
                  test_jacobian_pix << std::endl;
 
-
-
-
     //================================= projectPoint Complete
 
-
 //    //distortion
 //    distortion2;
 
@@ -332,7 +313,6 @@ int main(int argc, char** argv)
 //    //3Dpoint
 //    project_point_normalized_test;
 
-
     Eigen::Vector2s point2D_test5;
     point2D_test5 = pinhole::projectPoint(k_test2,
                                           distortion2,
@@ -393,7 +373,6 @@ int main(int argc, char** argv)
     std::cout << "\n-->Jacobian" << std::endl <<
                  jacobian_test4 << std::endl;
 
-
     /////////////////////////////
 
 //    //correction
@@ -414,8 +393,6 @@ int main(int argc, char** argv)
     std::cout << std::endl << "First function output" << std::endl;
     std::cout << "x: " << point3D_backproj5[0] << "; y: " << point3D_backproj5[1] << "; z: " << point3D_backproj5[2] << std::endl;
 
-
-
         //jacobian
     Eigen::Vector3s point3D_backproj4;
     Eigen::MatrixXs jacobian_backproj2(3,2);
@@ -435,20 +412,12 @@ int main(int argc, char** argv)
     std::cout << "\n--> Jacobian - depth" << std::endl <<
                  depth_jacobian2[0] << " " << depth_jacobian2[1] << " " << depth_jacobian2[2] << " " << std::endl;
 
-
-
-
     Eigen::Matrix2s test_jacobian_complete;
     test_jacobian_complete =  jacobian_test4 * jacobian_backproj2;
 
     std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
                  test_jacobian_complete << std::endl;
 
-
-
-
-
-
     /* Test */
     std::cout << "\n\n\n\nTEST\n" << std::endl;
 
@@ -476,8 +445,6 @@ int main(int argc, char** argv)
     Eigen::Vector2s test_point2D = {123,321};
     std::cout << "\ntest_point2D ORIGINAL:\n" << test_point2D << std::endl;
 
-
-
     test_point2D = pinhole::depixellizePoint(K_params,
                                              test_point2D);
     std::cout << "\ntest_point2D DEPIXELIZED:\n" << test_point2D << std::endl;
@@ -488,8 +455,6 @@ int main(int argc, char** argv)
                                                                 2);
     std::cout << "\ntest_point3D BACKPROJECTED:\n" << test_point3D << std::endl;
 
-
-
     test_point2D = pinhole::projectPointToNormalizedPlane(test_point3D);
     std::cout << "\ntest_point2D NORMALIZED:\n" << test_point2D << std::endl;
     test_point2D = pinhole::distortPoint(distortion_vector,
@@ -501,9 +466,3 @@ int main(int argc, char** argv)
 
 }
 
-
-
-
-
-
-
diff --git a/examples/test_sh_ptr.cpp b/src/examples/test_sh_ptr.cpp
similarity index 99%
rename from examples/test_sh_ptr.cpp
rename to src/examples/test_sh_ptr.cpp
index 74bdd8bd7..294555ed1 100644
--- a/examples/test_sh_ptr.cpp
+++ b/src/examples/test_sh_ptr.cpp
@@ -39,7 +39,6 @@ class c; // constraint
 class M; // map
 class L; // landmark
 
-
 //////////////////////////////////////////////////////////////////////////////////
 // DECLARE WOLF TREE
 
@@ -318,8 +317,6 @@ class M : public enable_shared_from_this<M>
         shared_ptr<L> add_L(shared_ptr<L> _L);
 };
 
-
-
 class L : public enable_shared_from_this<L>
 {
     private:
@@ -522,7 +519,6 @@ void c::remove()
     }
 }
 
-
 shared_ptr<L> M::add_L(shared_ptr<L> _L)
 {
     L_list_.push_back(_L);
@@ -544,8 +540,6 @@ void L::remove()
     }
 }
 
-
-
 }
 
 //////////////////////////////////////////////////////////////////////////////////
@@ -770,7 +764,6 @@ int main()
     cout<<endl;
     print_c(Pp);
 
-
     //------------------------------------------------------------------
     // Several tests. Uncomment the desired test.
     // Run only one test at a time, otherwise you'll get segfaults!
@@ -800,4 +793,3 @@ int main()
     return 1;
 }
 
-
diff --git a/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
similarity index 96%
rename from examples/test_simple_AHP.cpp
rename to src/examples/test_simple_AHP.cpp
index 6a3279326..818250e17 100644
--- a/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -5,15 +5,15 @@
  *      \author: jtarraso
  */
 
-#include "wolf.h"
-#include "frame_base.h"
-#include "pinhole_tools.h"
-#include "sensor_camera.h"
-#include "rotations.h"
-#include "capture_image.h"
-#include "landmark_AHP.h"
-#include "constraint_AHP.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/wolf.h"
+#include "base/frame_base.h"
+#include "base/pinhole_tools.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/rotations.h"
+#include "base/capture/capture_image.h"
+#include "base/landmark/landmark_AHP.h"
+#include "base/constraint/constraint_AHP.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // Vision utils
 #include <vision_utils/vision_utils.h>
@@ -61,12 +61,10 @@
  *      0   160  320  480  640
  *      +----+----+----+----+
  *                |
- *                |
  *                | 320
  *                |
  *                *
  *
- *
  *      projected pixels:
  *      p0 = (320,240) // at optical axis or relation 1:0
  *      p1 = ( 0 ,240) // at 45 deg or relation 1:1
@@ -129,7 +127,6 @@ int main(int argc, char** argv)
 //    std::cout << "lmk hmg in C frame: " << lmk_hmg_c.transpose() << std::endl;
     // ============================================================================================================
 
-
     // Captures------------------
     cv::Mat cv_image;
     cv_image.zeros(2,2,0);
@@ -199,7 +196,6 @@ int main(int argc, char** argv)
     FeaturePointImagePtr feat_4 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity());
     image_2->addFeature(feat_4);
 
-
     // New landmark with measured pixels from kf2 (anchor) kf3 and kf4 (measurements)
     Scalar unknown_distance = 2; // the real distance is 1
     Matrix3s K = camera->getIntrinsicMatrix();
@@ -241,7 +237,6 @@ int main(int argc, char** argv)
 
     CeresManager ceres_manager(problem, ceres_options);
 
-
     std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
     std::cout << summary << std::endl;
 
@@ -260,18 +255,3 @@ int main(int argc, char** argv)
 
 }
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
similarity index 95%
rename from examples/test_sort_keyframes.cpp
rename to src/examples/test_sort_keyframes.cpp
index a1e908a61..967d37006 100644
--- a/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -6,10 +6,10 @@
  */
 
 // Wolf includes
-#include "../wolf.h"
-#include "../problem.h"
-#include "../frame_base.h"
-#include "../trajectory_base.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/frame_base.h"
+#include "base/trajectory_base.h"
 
 // STL includes
 #include <list>
@@ -75,7 +75,5 @@ int main()
 
     printFrames(problem_ptr);
 
-
-
     return 0;
 }
diff --git a/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
similarity index 98%
rename from examples/test_sparsification.cpp
rename to src/examples/test_sparsification.cpp
index ea8113d1b..41b3b8eae 100644
--- a/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -14,10 +14,10 @@
 #include <queue>
 
 //Wolf includes
-#include "capture_void.h"
-#include "feature_odom_2D.h"
-#include "constraint_base.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/feature/feature_odom_2D.h"
+#include "base/constraint/constraint_base.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
@@ -297,7 +297,6 @@ int main(int argc, char** argv)
 			// covariance
         bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS
 
-
 	//		t1 = clock();
 	//		double t_sigma_manual = 0;
 	//		t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
diff --git a/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
similarity index 90%
rename from examples/test_state_quaternion.cpp
rename to src/examples/test_state_quaternion.cpp
index ec23b9e0f..4d2aa60cc 100644
--- a/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -5,10 +5,9 @@
  *      \author: jsola
  */
 
-
-#include "frame_base.h"
-#include "state_quaternion.h"
-#include "time_stamp.h"
+#include "base/frame_base.h"
+#include "base/state_quaternion.h"
+#include "base/time_stamp.h"
 
 #include <iostream>
 
@@ -36,6 +35,5 @@ int main (void)
 
     std::cout << "Done" << std::endl;
 
-
     return 1;
 }
diff --git a/examples/test_tracker_ORB.cpp b/src/examples/test_tracker_ORB.cpp
similarity index 99%
rename from examples/test_tracker_ORB.cpp
rename to src/examples/test_tracker_ORB.cpp
index f695a3cb4..536e06d62 100644
--- a/examples/test_tracker_ORB.cpp
+++ b/src/examples/test_tracker_ORB.cpp
@@ -11,7 +11,7 @@
 #include <matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h>
 
 //Wolf
-#include "../processor_tracker_landmark_image.h"
+#include "base/processor/processor_tracker_landmark_image.h"
 
 int main(int argc, char** argv)
 {
@@ -244,7 +244,6 @@ int main(int argc, char** argv)
             current_keypoints.clear();
             current_descriptors.release();
 
-
             std::cout << "\nAFTER THE ADVANCE" << std::endl;
 //            for(unsigned int i = 0; i < target_keypoints.size(); i++)
 //            {
diff --git a/examples/test_virtual_hierarchy.cpp b/src/examples/test_virtual_hierarchy.cpp
similarity index 100%
rename from examples/test_virtual_hierarchy.cpp
rename to src/examples/test_virtual_hierarchy.cpp
diff --git a/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
similarity index 99%
rename from examples/test_wolf_autodiffwrapper.cpp
rename to src/examples/test_wolf_autodiffwrapper.cpp
index 5728739a8..a8de6a82b 100644
--- a/examples/test_wolf_autodiffwrapper.cpp
+++ b/src/examples/test_wolf_autodiffwrapper.cpp
@@ -12,8 +12,8 @@
 
 //Wolf includes
 #include "wolf_manager.h"
-#include "capture_void.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 int main(int argc, char** argv) 
 {
@@ -57,8 +57,6 @@ int main(int argc, char** argv)
     CeresManager* ceres_manager_ceres_diff = new CeresManager(wolf_problem_ceres_diff, ceres_options, false);
     CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_wolf_diff, ceres_options, true);
 
-
-
     // load graph from .txt
     offLineFile_.open(file_path_.c_str(), std::ifstream::in);
     if (offLineFile_.is_open())
diff --git a/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
similarity index 92%
rename from examples/test_wolf_factories.cpp
rename to src/examples/test_wolf_factories.cpp
index 3b2b68c18..4108864a7 100644
--- a/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -5,27 +5,26 @@
  *      \author: jsola
  */
 
-#include <processor_IMU.h>
-#include <sensor_GPS_fix.h>
-#include "../hardware_base.h"
-#include "../sensor_camera.h"
-#include "../sensor_odom_2D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_GPS_fix.h"
+#include "base/hardware_base.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/sensor/sensor_odom_2D.h"
 #include "../sensor_imu.h"
 //#include "../sensor_gps.h"
 
-#include "../processor_odom_2D.h"
-#include "../processor_odom_3D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/processor/processor_odom_3D.h"
 #include "../processor_image_feature.h"
 
-#include "../problem.h"
+#include "base/problem.h"
 
-#include "../factory.h"
+#include "base/factory.h"
 
 #include <iostream>
 #include <iomanip>
 #include <cstdlib>
 
-
 int main(void)
 {
     using namespace wolf;
@@ -34,8 +33,6 @@ int main(void)
     using std::make_shared;
     using std::static_pointer_cast;
 
-
-
     //==============================================================================================
     std::string wolf_root       = _WOLF_ROOT_DIR;
     std::string wolf_config     = wolf_root + "/src/examples";
@@ -105,7 +102,6 @@ int main(void)
             << " | name: " << setw(17) << prc->getName()
             << " | bound to sensor " << setw(2) << prc->getSensorPtr()->id() << ": " << prc->getSensorPtr()->getName() << endl;
 
-
     return 0;
 }
 
diff --git a/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
similarity index 99%
rename from examples/test_wolf_imported_graph.cpp
rename to src/examples/test_wolf_imported_graph.cpp
index fc8129dc5..cbfe23b78 100644
--- a/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -12,8 +12,8 @@
 
 //Wolf includes
 #include "wolf_manager.h"
-#include "capture_void.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
@@ -28,7 +28,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr
 
   original.makeCompressed();
 }
-}
 
 int main(int argc, char** argv) 
 {
@@ -76,8 +75,6 @@ int main(int argc, char** argv)
     CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options);
     CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options);
 
-
-
     // load graph from .txt
     offLineFile_.open(file_path_.c_str(), std::ifstream::in);
     if (offLineFile_.is_open())
@@ -362,7 +359,6 @@ int main(int argc, char** argv)
 //        std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
 //        std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
 
-
         //jacobian
         xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr();
         yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1);
diff --git a/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp
similarity index 87%
rename from examples/test_wolf_logging.cpp
rename to src/examples/test_wolf_logging.cpp
index 0f92079cd..b516acd09 100644
--- a/examples/test_wolf_logging.cpp
+++ b/src/examples/test_wolf_logging.cpp
@@ -5,8 +5,8 @@
  * \author: Jeremie Deray
  */
 
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
 
 int main(int, char*[])
 {
diff --git a/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
similarity index 99%
rename from examples/test_wolf_prunning.cpp
rename to src/examples/test_wolf_prunning.cpp
index 3191326c0..13b3dd86d 100644
--- a/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -12,9 +12,9 @@
 
 //Wolf includes
 #include "wolf_manager.h"
-#include "capture_void.h"
-#include "constraint_base.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_void.h"
+#include "base/constraint/constraint_base.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // EIGEN
 //#include <Eigen/CholmodSupport>
@@ -31,7 +31,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr
 
   original.makeCompressed();
 }
-}
 
 int main(int argc, char** argv) 
 {
@@ -403,7 +402,6 @@ int main(int argc, char** argv)
 //        std::cout << "Sigma_34" << std::endl << Sigma_34 << std::endl;
 //        std::cout << "Sigma_44" << std::endl << Sigma_44 << std::endl;
 
-
         // jacobians
         ((ConstraintAnalytic*)(*c_it))->evaluatePureJacobians(jacobians);
         Eigen::MatrixXs& J1 = jacobians[0];
@@ -434,7 +432,6 @@ int main(int argc, char** argv)
                                               J4 * Sigma_34.transpose() * J3.transpose() +
                                               J4 * Sigma_44 * J4.transpose()) ).determinant() );
 
-
 //        std::cout << "part = " << std::endl << (J1 * Sigma_11 * J1.transpose() +
 //                                                  J1 * Sigma_12 * J2.transpose() +
 //                                                  J1 * Sigma_13 * J3.transpose() +
diff --git a/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp
similarity index 92%
rename from examples/test_wolf_root.cpp
rename to src/examples/test_wolf_root.cpp
index f6dc51ce9..a2e3ccef5 100644
--- a/examples/test_wolf_root.cpp
+++ b/src/examples/test_wolf_root.cpp
@@ -6,7 +6,7 @@
  */
 
 //Wolf
-#include "wolf.h"
+#include "base/wolf.h"
 
 //std
 #include <iostream>
diff --git a/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp
similarity index 100%
rename from examples/test_wolf_tree.cpp
rename to src/examples/test_wolf_tree.cpp
diff --git a/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
similarity index 98%
rename from examples/test_yaml.cpp
rename to src/examples/test_yaml.cpp
index 56a33fe1d..cfd03d18d 100644
--- a/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -5,10 +5,10 @@
  *      \author: jsola
  */
 
-#include "pinhole_tools.h"
+#include "base/pinhole_tools.h"
 #include "yaml/yaml_conversion.h"
 #include "processor_image_feature.h"
-#include "factory.h"
+#include "base/factory.h"
 
 #include <yaml-cpp/yaml.h>
 
@@ -17,7 +17,6 @@
 #include <iostream>
 #include <fstream>
 
-
 int main()
 {
 
@@ -69,8 +68,6 @@ int main()
     else
         std::cout << "Bad configuration file. No sensor type found." << std::endl;
 
-
-
 //    // Processor Image parameters
 //
 //    ProcessorParamsImage p;
@@ -93,6 +90,5 @@ int main()
 //        p.min_features_for_keyframe   = alg["minimum features for new keyframe"].as<unsigned int>();
 //    }
 
-
     return 0;
 }
diff --git a/examples/test_yaml_conversions.cpp b/src/examples/test_yaml_conversions.cpp
similarity index 97%
rename from examples/test_yaml_conversions.cpp
rename to src/examples/test_yaml_conversions.cpp
index d6393d8b8..78cf30da7 100644
--- a/examples/test_yaml_conversions.cpp
+++ b/src/examples/test_yaml_conversions.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "../yaml/yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
 
 #include <yaml-cpp/yaml.h>
 
@@ -14,13 +14,11 @@
 #include <iostream>
 //#include <fstream>
 
-
 int main()
 {
 
     using namespace Eigen;
 
-
     std::cout << "\nTrying different yaml specs for matrix..." << std::endl;
 
     YAML::Node mat_sized_23, mat_sized_33, mat_sized_bad, mat_23, mat_33, mat_bad;
diff --git a/examples/vision_utils_active_search.yaml b/src/examples/vision_utils_active_search.yaml
similarity index 100%
rename from examples/vision_utils_active_search.yaml
rename to src/examples/vision_utils_active_search.yaml
diff --git a/src/feature/feature_GPS_fix.cpp b/src/feature/feature_GPS_fix.cpp
index ca370b59b..d4b5a12f2 100644
--- a/src/feature/feature_GPS_fix.cpp
+++ b/src/feature/feature_GPS_fix.cpp
@@ -1,4 +1,4 @@
-#include "feature_GPS_fix.h"
+#include "base/feature/feature_GPS_fix.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_GPS_pseudorange.cpp b/src/feature/feature_GPS_pseudorange.cpp
index 37bbfd8fa..7979f30e4 100644
--- a/src/feature/feature_GPS_pseudorange.cpp
+++ b/src/feature/feature_GPS_pseudorange.cpp
@@ -1,4 +1,4 @@
-#include "feature_GPS_pseudorange.h"
+#include "base/feature/feature_GPS_pseudorange.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index 796f4834a..0f811cff5 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -1,4 +1,4 @@
-#include "feature_IMU.h"
+#include "base/feature/feature_IMU.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 6459ffc24..314457be2 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -1,6 +1,6 @@
-#include "feature_base.h"
-#include "constraint_base.h"
-#include "capture_base.h"
+#include "base/feature/feature_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/capture/capture_base.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_corner_2D.cpp b/src/feature/feature_corner_2D.cpp
index 59db17e3b..27803ef4a 100644
--- a/src/feature/feature_corner_2D.cpp
+++ b/src/feature/feature_corner_2D.cpp
@@ -1,5 +1,5 @@
 
-#include "feature_corner_2D.h"
+#include "base/feature/feature_corner_2D.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index c33d5fdec..c2b8d9ed1 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -1,4 +1,4 @@
-#include "feature_diff_drive.h"
+#include "base/feature/feature_diff_drive.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_line_2D.cpp b/src/feature/feature_line_2D.cpp
index d6c54f4e9..c69ba2c1d 100644
--- a/src/feature/feature_line_2D.cpp
+++ b/src/feature/feature_line_2D.cpp
@@ -1,5 +1,5 @@
 
-#include "feature_corner_2D.h"
+#include "base/feature/feature_corner_2D.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
index 23c28cc7b..4f37086f9 100644
--- a/src/feature/feature_odom_2D.cpp
+++ b/src/feature/feature_odom_2D.cpp
@@ -1,4 +1,4 @@
-#include "feature_odom_2D.h"
+#include "base/feature/feature_odom_2D.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_point_image.cpp b/src/feature/feature_point_image.cpp
index 5e240fcbe..e7d3781b6 100644
--- a/src/feature/feature_point_image.cpp
+++ b/src/feature/feature_point_image.cpp
@@ -1,5 +1,5 @@
 
-#include "feature_point_image.h"
+#include "base/feature/feature_point_image.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_polyline_2D.cpp b/src/feature/feature_polyline_2D.cpp
index c2a19ff5a..0f14a701c 100644
--- a/src/feature/feature_polyline_2D.cpp
+++ b/src/feature/feature_polyline_2D.cpp
@@ -5,7 +5,7 @@
  *      \author: jvallve
  */
 
-#include "feature_polyline_2D.h"
+#include "base/feature/feature_polyline_2D.h"
 
 namespace wolf
 {
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index 35193d2b0..15d40e60a 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.cpp
@@ -1,5 +1,4 @@
-#include "feature_pose.h"
-
+#include "base/feature/feature_pose.h"
 
 namespace wolf {
 
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 4f0f619fe..5c1b7fee2 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -1,11 +1,11 @@
 
-#include "frame_base.h"
-#include "constraint_base.h"
-#include "trajectory_base.h"
-#include "capture_base.h"
-#include "state_block.h"
-#include "state_angle.h"
-#include "state_quaternion.h"
+#include "base/frame_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/trajectory_base.h"
+#include "base/capture/capture_base.h"
+#include "base/state_block.h"
+#include "base/state_angle.h"
+#include "base/state_quaternion.h"
 
 namespace wolf {
 
@@ -226,8 +226,6 @@ Eigen::MatrixXs FrameBase::getCovariance() const
     return getProblem()->getFrameCovariance(shared_from_this());
 }
 
-
-
 FrameBasePtr FrameBase::getPreviousFrame() const
 {
     //std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
@@ -386,7 +384,7 @@ FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
 
 } // namespace wolf
 
-#include "factory.h"
+#include "base/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_base.cpp
index b2171c3cd..334900512 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -1,6 +1,5 @@
-#include "hardware_base.h"
-#include "sensor_base.h"
-
+#include "base/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 78b2bdb32..f1a683978 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -1,7 +1,7 @@
-#include "landmark_AHP.h"
+#include "base/landmark/landmark_AHP.h"
 
-#include "state_homogeneous_3D.h"
-#include "factory.h"
+#include "base/state_homogeneous_3D.h"
+#include "base/factory.h"
 #include "yaml/yaml_conversion.h"
 
 namespace wolf {
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 830ae2940..1ae594cb3 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -1,9 +1,9 @@
 
-#include "landmark_base.h"
-#include "constraint_base.h"
-#include "map_base.h"
-#include "state_block.h"
-#include "yaml/yaml_conversion.h"
+#include "base/landmark/landmark_base.h"
+#include "base/constraint/constraint_base.h"
+#include "base/map_base.h"
+#include "base/state_block.h"
+#include "base/yaml/yaml_conversion.h"
 
 namespace wolf {
 
@@ -120,7 +120,6 @@ void LandmarkBase::removeStateBlocks()
     }
 }
 
-
 Eigen::VectorXs LandmarkBase::getState() const
 {
     SizeEigen size = 0;
@@ -153,7 +152,6 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const
         }
 }
 
-
 YAML::Node LandmarkBase::saveToYaml() const
 {
     YAML::Node node;
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
index a1ef7ed10..6cef90a38 100644
--- a/src/landmark/landmark_container.cpp
+++ b/src/landmark/landmark_container.cpp
@@ -1,6 +1,6 @@
 
-#include "landmark_container.h"
-#include "state_block.h"
+#include "base/landmark/landmark_container.h"
+#include "base/state_block.h"
 
 namespace wolf {
 
diff --git a/src/landmark/landmark_corner_2D.cpp b/src/landmark/landmark_corner_2D.cpp
index 200b5a7cc..558292f90 100644
--- a/src/landmark/landmark_corner_2D.cpp
+++ b/src/landmark/landmark_corner_2D.cpp
@@ -1,5 +1,5 @@
 
-#include "landmark_corner_2D.h"
+#include "base/landmark/landmark_corner_2D.h"
 
 namespace wolf {
 
@@ -14,7 +14,6 @@ LandmarkCorner2D::~LandmarkCorner2D()
     //
 }
 
-
 Scalar LandmarkCorner2D::getAperture() const
 {
     return descriptor_(0);
diff --git a/src/landmark/landmark_line_2D.cpp b/src/landmark/landmark_line_2D.cpp
index 80c916932..aa2a14d9f 100644
--- a/src/landmark/landmark_line_2D.cpp
+++ b/src/landmark/landmark_line_2D.cpp
@@ -1,5 +1,5 @@
 
-#include "landmark_line_2D.h"
+#include "base/landmark/landmark_line_2D.h"
 
 namespace wolf {
 
@@ -43,8 +43,6 @@ void LandmarkLine2D::updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s
     //compute all necessary scalar products. 
     //s_p1q1 = p1p2.dot()
     
-    
-    
 }
 
 const Eigen::Vector3s & LandmarkLine2D::point1() const
diff --git a/src/landmark/landmark_point_3D.h b/src/landmark/landmark_point_3D.h
index cb29852b8..7ee1c0507 100644
--- a/src/landmark/landmark_point_3D.h
+++ b/src/landmark/landmark_point_3D.h
@@ -1,9 +1,8 @@
 #ifndef LANDMARK_POINT_3D_H
 #define LANDMARK_POINT_3D_H
 
-
 //Wolf includes
-#include "landmark_base.h"
+#include "base/landmark/landmark_base.h"
 
 // Vision Utils includes
 #include <vision_utils/vision_utils.h>
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index f38680318..3e1fcf45c 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -5,19 +5,18 @@
  *      \author: jvallve
  */
 
-#include "feature_polyline_2D.h"
-#include "landmark_polyline_2D.h"
-#include "local_parametrization_polyline_extreme.h"
-#include "constraint_point_2D.h"
-#include "constraint_point_to_line_2D.h"
-#include "state_block.h"
-#include "factory.h"
-#include "yaml/yaml_conversion.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/local_parametrization_polyline_extreme.h"
+#include "base/constraint/constraint_point_2D.h"
+#include "base/constraint/constraint_point_to_line_2D.h"
+#include "base/state_block.h"
+#include "base/factory.h"
+#include "base/yaml/yaml_conversion.h"
 
 namespace wolf
 {
 
-
 LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_extreme, const bool _last_extreme, unsigned int _first_id, LandmarkClassification _class) :
         LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_id_(_first_id), first_defined_(_first_extreme), last_defined_(_last_extreme), closed_(false), classification_(_class)
 {
@@ -337,7 +336,6 @@ void LandmarkPolyline2D::removeStateBlocks()
     LandmarkBase::removeStateBlocks();
 }
 
-
 // static
 LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
 {
diff --git a/src/local_parametrization_base.cpp b/src/local_parametrization_base.cpp
index 7b8552f39..e2bc1cf4e 100644
--- a/src/local_parametrization_base.cpp
+++ b/src/local_parametrization_base.cpp
@@ -1,5 +1,4 @@
-#include "local_parametrization_base.h"
-
+#include "base/local_parametrization_base.h"
 
 namespace wolf {
 
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 482178155..14abaecd1 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/src/local_parametrization_homogeneous.cpp
@@ -5,9 +5,9 @@
  *      Author: jsola
  */
 
-#include "local_parametrization_homogeneous.h"
+#include "base/local_parametrization_homogeneous.h"
 #include "iostream"
-#include "rotations.h" // we use quaternion algebra here
+#include "base/rotations.h" // we use quaternion algebra here
 
 namespace wolf {
 
diff --git a/src/local_parametrization_polyline_extreme.cpp b/src/local_parametrization_polyline_extreme.cpp
index 43f64d069..da318cafb 100644
--- a/src/local_parametrization_polyline_extreme.cpp
+++ b/src/local_parametrization_polyline_extreme.cpp
@@ -1,6 +1,6 @@
-#include "local_parametrization_polyline_extreme.h"
-#include "state_block.h"
-#include "rotations.h"
+#include "base/local_parametrization_polyline_extreme.h"
+#include "base/state_block.h"
+#include "base/rotations.h"
 
 namespace wolf {
 
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index 46e9ca9f8..d2af8f544 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -1,6 +1,6 @@
 
-#include "local_parametrization_quaternion.h"
-#include "rotations.h"
+#include "base/local_parametrization_quaternion.h"
+#include "base/rotations.h"
 
 #include <iostream>
 namespace wolf {
@@ -57,7 +57,6 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec
     return true;
 }
 
-
 ////////// GLOBAL PERTURBATION //////////////
 
 template <>
@@ -110,6 +109,4 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::Ve
     return true;
 }
 
-
-
 } // namespace wolf
diff --git a/src/map_base.cpp b/src/map_base.cpp
index ebcf2d0d8..5e8008a6a 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -1,8 +1,8 @@
 
 // wolf
-#include "map_base.h"
-#include "landmark_base.h"
-#include "factory.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
+#include "base/factory.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>
@@ -14,8 +14,6 @@
 #include <iostream>
 #include <sstream>
 
-
-
 namespace wolf {
 
 MapBase::MapBase() :
@@ -85,7 +83,6 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
     }
     emitter << YAML::EndSeq << YAML::EndMap;
 
-
     std::ofstream fout(_map_file_yaml);
     fout << emitter.c_str();
     fout.close();
diff --git a/src/motion_buffer.cpp b/src/motion_buffer.cpp
index dbdb539f6..20d4fc260 100644
--- a/src/motion_buffer.cpp
+++ b/src/motion_buffer.cpp
@@ -1,4 +1,4 @@
-#include "motion_buffer.h"
+#include "base/motion_buffer.h"
 namespace wolf
 {
 
@@ -60,7 +60,6 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
     jacobian_calib_.resize(_delta_cov_s, _calib_s);
 }
 
-
 ////////////////////////////////////////////////////////////////////////////////////////
 
 MotionBuffer::MotionBuffer(SizeEigen _data_size,
@@ -221,6 +220,5 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
     }
 }
 
-
 }
 
diff --git a/src/node_base.cpp b/src/node_base.cpp
index f6f4ceb53..24cb56c20 100644
--- a/src/node_base.cpp
+++ b/src/node_base.cpp
@@ -1,4 +1,4 @@
-#include "node_base.h"
+#include "base/node_base.h"
 
 namespace wolf {
 
diff --git a/src/problem.cpp b/src/problem.cpp
index fec23b174..40ef35f2f 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -1,20 +1,20 @@
 // wolf includes
-#include "problem.h"
-#include "hardware_base.h"
-#include "trajectory_base.h"
-#include "map_base.h"
-#include "sensor_base.h"
-#include "factory.h"
-#include "sensor_factory.h"
-#include "processor_factory.h"
-#include "constraint_base.h"
-#include "state_block.h"
-#include "processor_motion.h"
-#include "sensor_GPS.h"
-
-#include "processor_tracker.h"
+#include "base/problem.h"
+#include "base/hardware_base.h"
+#include "base/trajectory_base.h"
+#include "base/map_base.h"
+#include "base/sensor/sensor_base.h"
+#include "base/factory.h"
+#include "base/sensor/sensor_factory.h"
+#include "base/processor/processor_factory.h"
+#include "base/constraint/constraint_base.h"
+#include "base/state_block.h"
+#include "base/processor/processor_motion.h"
+#include "base/sensor/sensor_GPS.h"
+
+#include "base/processor/processor_tracker.h"
 //#include "processors/processor_tracker_feature_trifocal.h"
-#include "capture_pose.h"
+#include "base/capture/capture_pose.h"
 
 // IRI libs includes
 
@@ -30,7 +30,6 @@ namespace
 std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
 }
 
-
 Problem::Problem(const std::string& _frame_structure) :
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
@@ -214,7 +213,6 @@ void Problem::clearProcessorMotion()
     processor_motion_ptr_.reset();
 }
 
-
 FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
                                    FrameType _frame_key_type, //
                                    const Eigen::VectorXs& _frame_state, //
@@ -264,7 +262,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
         state = zeroState();
 }
 
-
 void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
 {
     assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
@@ -677,8 +674,6 @@ StateBlockList& Problem::getStateBlockList()
     return state_block_list_;
 }
 
-
-
 FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
 {
     if ( ! prior_is_set_ )
@@ -1401,6 +1396,4 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool
         print(0, constr_by, metric, state_blocks);
 }
 
-
-
 } // namespace wolf
diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp
index ad30cff7e..ffb3f7d0e 100644
--- a/src/processor/processor_GPS.cpp
+++ b/src/processor/processor_GPS.cpp
@@ -2,11 +2,11 @@
 // Created by ptirindelli on 16/12/15.
 //
 
-#include "constraint_GPS_pseudorange_2D.h"
-#include "feature_GPS_pseudorange.h"
-#include "processor_GPS.h"
+#include "base/constraint/constraint_GPS_pseudorange_2D.h"
+#include "base/feature/feature_GPS_pseudorange.h"
+#include "base/processor/processor_GPS.h"
 
-#include "capture_GPS.h"
+#include "base/capture/capture_GPS.h"
 
 namespace wolf
 {
@@ -68,7 +68,6 @@ ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const Pro
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
 #include "processor_factory.h"
 namespace wolf {
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index e69126258..568b61a78 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -1,7 +1,7 @@
 // wolf
-#include "processor_IMU.h"
-#include "constraint_IMU.h"
-#include "IMU_tools.h"
+#include "base/processor/processor_IMU.h"
+#include "base/constraint/constraint_IMU.h"
+#include "base/IMU_tools.h"
 
 namespace wolf {
 
@@ -329,9 +329,8 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index ea4bdc4e8..aa259d706 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -1,7 +1,7 @@
-#include "processor_base.h"
-#include "processor_motion.h"
-#include "capture_base.h"
-#include "frame_base.h"
+#include "base/processor/processor_base.h"
+#include "base/processor/processor_motion.h"
+#include "base/capture/capture_base.h"
+#include "base/frame_base.h"
 
 namespace wolf {
 
@@ -73,8 +73,6 @@ void ProcessorBase::remove()
     }
 }
 
-
-
 /////////////////////////////////////////////////////////////////////////////////////////
 
 void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
@@ -186,6 +184,4 @@ bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const
     return pass;
 }
 
-
-
 } // namespace wolf
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
index 10ded0c0b..58e3550ab 100644
--- a/src/processor/processor_capture_holder.cpp
+++ b/src/processor/processor_capture_holder.cpp
@@ -6,7 +6,7 @@
  */
 
 //Wolf includes
-#include "processor_capture_holder.h"
+#include "base/processor/processor_capture_holder.h"
 
 namespace wolf {
 
@@ -121,7 +121,6 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
 
 //  return capt;
 
-
   return buffer_.getCapture(_ts);
 }
 
@@ -146,7 +145,7 @@ ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name,
 } // namespace wolf
 
 // Register in the ProcessorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("CAPTURE HOLDER", ProcessorCaptureHolder)
 } // namespace wolf
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index dba867cc3..423e429db 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -1,13 +1,13 @@
-#include "processor_diff_drive.h"
+#include "base/processor/processor_diff_drive.h"
 
-#include "../sensors/sensor_diff_drive.h"
+#include "base/sensor/sensor_diff_drive.h"
 
-#include "../captures/capture_wheel_joint_position.h"
-#include "../captures/capture_velocity.h"
+#include "base/capture/capture_wheel_joint_position.h"
+#include "base/capture/capture_velocity.h"
 
-#include "../rotations.h"
-#include "../constraint_odom_2D.h"
-#include "../features/feature_diff_drive.h"
+#include "base/rotations.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/feature/feature_diff_drive.h"
 
 namespace wolf
 {
@@ -259,7 +259,7 @@ ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
 } // namespace wolf
 
 // Register in the ProcessorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
 } // namespace wolf
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index f705e368f..4901170c2 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -1,4 +1,4 @@
-#include "processor_frame_nearest_neighbor_filter.h"
+#include "base/processor/processor_frame_nearest_neighbor_filter.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp
index 160185238..6953cd103 100644
--- a/src/processor/processor_loopclosure_base.cpp
+++ b/src/processor/processor_loopclosure_base.cpp
@@ -5,7 +5,7 @@
  *      \author: Tessa Johanna
  */
 
-#include "processor_loopclosure_base.h"
+#include "base/processor/processor_loopclosure_base.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0abab2143..0fe59c59a 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,4 +1,4 @@
-#include "processor_motion.h"
+#include "base/processor/processor_motion.h"
 namespace wolf
 {
 
@@ -88,7 +88,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
             // and give the part of the buffer before the new keyframe to the capture for the KF callback
             existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
 
-
             // interpolate individual delta
             if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
             {
@@ -100,7 +99,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
                 capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
             }
 
-
             // create motion feature and add it to the capture
             auto new_feature = emplaceFeature(capture_for_keyframe_callback);
 
@@ -131,7 +129,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
             break;
         }
 
-
         case RUNNING_WITH_PACK_AFTER_ORIGIN :
         {
             // extract pack elements
@@ -185,8 +182,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
             break;
         }
 
-
-
         default :
             break;
     }
@@ -256,7 +251,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
     postProcess();
 }
 
-
 bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
 {
     CaptureMotionPtr capture_motion;
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 19818dffc..c375d52fa 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -1,4 +1,4 @@
-#include "processor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
 namespace wolf
 {
 
@@ -184,13 +184,10 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const
     return prc_ptr;
 }
 
-
-
 }
 
-
 // Register in the ProcessorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D)
 } // namespace wolf
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 49da6be81..4f6d75911 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -1,4 +1,4 @@
-#include "processor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
 namespace wolf
 {
 
@@ -15,7 +15,6 @@ ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DP
             jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_);
         }
 
-
 ProcessorOdom3D::~ProcessorOdom3D()
 {
 }
@@ -200,7 +199,6 @@ Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _motion_s
 //    assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
 //    assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
 
-
     using namespace Eigen;
     // Interpolate between motion_ref and motion, as in:
     //
@@ -347,9 +345,8 @@ void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _
 
 }
 
-
 // Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("ODOM 3D", ProcessorOdom3D)
 } // namespace wolf
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 630a856e7..f8138e1fc 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -6,7 +6,7 @@
  */
 
 // wolf
-#include "processor_tracker.h"
+#include "base/processor/processor_tracker.h"
 
 // std
 #include <cmath>
@@ -233,7 +233,6 @@ void ProcessorTracker::computeProcessingStep()
     else
         step = RUNNING;
 
-
     // Then combine with the existence (or not) of a keyframe callback pack
     switch (step)
     {
@@ -277,8 +276,5 @@ void ProcessorTracker::computeProcessingStep()
     }
 }
 
-
-
-
 } // namespace wolf
 
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 0ce945f30..cfb6aa984 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "processor_tracker_feature.h"
+#include "base/processor/processor_tracker_feature.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 387333fd7..0a8e7c7c2 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -5,8 +5,8 @@
  *      \author: jvallve
  */
 
-#include "processor_tracker_feature_corner.h"
-#include "feature_corner_2D.h"
+#include "base/processor/processor_tracker_feature_corner.h"
+#include "base/feature/feature_corner_2D.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index dc81126f9..6ce3d4fe1 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -5,8 +5,8 @@
  *      \author: jvallve
  */
 
-#include "processor_tracker_feature_dummy.h"
-#include "feature_base.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/feature/feature_base.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 1a860c217..6ec2b8aab 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -1,5 +1,5 @@
 // Wolf includes
-#include "processor_tracker_feature_image.h"
+#include "base/processor/processor_tracker_feature_image.h"
 
 // Vision utils
 #include <detectors.h>
@@ -382,7 +382,6 @@ void ProcessorTrackerFeatureImage::drawFeatures(cv::Mat _image)
     }
 }
 
-
 ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr)
 {
     ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(std::static_pointer_cast<ProcessorParamsTrackerFeatureImage>(_params));
@@ -390,10 +389,8 @@ ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique
     return prc_ptr;
 }
 
-
 } // namespace wolf
 
-
 // Register in the SensorFactory
 #include "processor_factory.h"
 namespace wolf {
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 7ffb99375..5557e10d6 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -1,11 +1,11 @@
 
 // wolf
-#include "processor_tracker_feature_trifocal.h"
+#include "processor/processor_tracker_feature_trifocal.h"
 
-#include "sensor_camera.h"
-#include "feature_point_image.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/feature/feature_point_image.h"
 #include "constraints/constraint_autodiff_trifocal.h"
-#include "capture_image.h"
+#include "base/capture/capture_image.h"
 
 // vision_utils
 #include <vision_utils.h>
@@ -18,7 +18,6 @@
 
 namespace wolf {
 
-
 //// DEBUG =====================================
 //debug_tTmp = clock();
 //WOLF_TRACE("======== DetectNewFeatures =========");
@@ -29,7 +28,6 @@ namespace wolf {
 //WOLF_TRACE("--> TIME: Detect New Features: detect ",debug_comp_time_);
 //// ===========================================
 
-
 // Constructor
 ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal) :
         ProcessorTrackerFeature("TRACKER FEATURE TRIFOCAL", _params_tracker_feature_trifocal ),
diff --git a/src/processor/processor_tracker_feature_trifocal.h b/src/processor/processor_tracker_feature_trifocal.h
index 9065c718c..dca3b791a 100644
--- a/src/processor/processor_tracker_feature_trifocal.h
+++ b/src/processor/processor_tracker_feature_trifocal.h
@@ -2,8 +2,8 @@
 #define _PROCESSOR_TRACKER_FEATURE_TRIFOCAL_H_
 
 //Wolf includes
-#include "../processor_tracker_feature.h"
-#include "../capture_image.h"
+#include "base/processor/processor_tracker_feature.h"
+#include "base/capture/capture_image.h"
 
 // Vision utils
 #include <vision_utils.h>
@@ -104,7 +104,6 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
          */
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
 
-
         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
         /** \brief advance pointers
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 8e3f5d0d9..08bbf8c52 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -5,12 +5,11 @@
  *      \author: jvallve
  */
 
-#include "processor_tracker_landmark.h"
-#include "map_base.h"
+#include "base/processor/processor_tracker_landmark.h"
+#include "base/map_base.h"
 
 #include <utility>
 
-
 namespace wolf
 {
 
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index e687e5fcd..522034204 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -1,5 +1,5 @@
-#include "processor_tracker_landmark_corner.h"
-#include "rotations.h"
+#include "base/processor/processor_tracker_landmark_corner.h"
+#include "base/rotations.h"
 
 namespace wolf
 {
@@ -422,7 +422,7 @@ ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(ProcessorParamsLa
 }        //namespace wolf
 
 // Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK CORNER", ProcessorTrackerLandmarkCorner)
 } // namespace wolf
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index e806c23e5..06c10b37f 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -5,9 +5,9 @@
  *      \author: jvallve
  */
 
-#include "processor_tracker_landmark_dummy.h"
-#include "landmark_corner_2D.h"
-#include "constraint_corner_2D.h"
+#include "base/processor/processor_tracker_landmark_dummy.h"
+#include "base/landmark/landmark_corner_2D.h"
+#include "base/constraint/constraint_corner_2D.h"
 
 namespace wolf
 {
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index da3f1c7b4..04bba415f 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -1,17 +1,17 @@
-#include "processor_tracker_landmark_image.h"
-
-#include "capture_image.h"
-#include "constraint_AHP.h"
-#include "feature_base.h"
-#include "feature_point_image.h"
-#include "frame_base.h"
-#include "logging.h"
-#include "map_base.h"
-#include "pinhole_tools.h"
-#include "problem.h"
-#include "sensor_camera.h"
-#include "state_block.h"
-#include "time_stamp.h"
+#include "base/processor/processor_tracker_landmark_image.h"
+
+#include "base/capture/capture_image.h"
+#include "base/constraint/constraint_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/sensor/sensor_camera.h"
+#include "base/state_block.h"
+#include "base/time_stamp.h"
 
 // vision_utils
 #include <detectors.h>
@@ -196,7 +196,6 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
 
                     _features_incoming_out.push_back(incoming_point_ptr);
 
-
                     _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
 
                     feat_lmk_found_.push_back(incoming_point_ptr);
@@ -302,7 +301,6 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
 
     point3D.normalize();
 
-
     vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
 
     LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensorPtr(), feat_point_image_ptr->getDescriptor());
@@ -330,7 +328,6 @@ ConstraintBasePtr ProcessorTrackerLandmarkImage::createConstraint(FeatureBasePtr
     }
 }
 
-
 // ==================================================================== My own functions
 
 void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorXs& _current_state,
@@ -361,7 +358,6 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
      * We use Eigen::Transform which is like using homogeneous transform matrices with a simpler API
      */
 
-
     // Assert frame is 3D with at least PQ
     assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
 
@@ -493,7 +489,6 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
     }
 }
 
-
 //namespace wolf{
 
 ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
@@ -505,7 +500,6 @@ ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _uniqu
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
 #include "processor_factory.h"
 namespace wolf {
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
index 0475ab31a..70efbcde6 100644
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ b/src/processor/processor_tracker_landmark_polyline.cpp
@@ -1,4 +1,4 @@
-#include "processor_tracker_landmark_polyline.h"
+#include "base/processor/processor_tracker_landmark_polyline.h"
 
 namespace wolf
 {
@@ -268,7 +268,6 @@ unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseL
             //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined())
             //    std::cout << "\tLIMITED min offset " << min_offset << std::endl;
 
-
         }
         // Match found for this feature
         if (best_match != nullptr)
@@ -444,7 +443,6 @@ Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s
      *      Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B.
      *      Checks if the angle BAAaux is >=  90º: (BAaux)² >= (BA)² + (AAaux)²
      *
-     *
      * ( Case B and A are defined is not point-to-line, is point to point -> assertion )
      *
      */
@@ -510,7 +508,6 @@ LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr
     //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
     //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
 
-
     FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
     // compute feature global pose
     Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
@@ -1017,7 +1014,7 @@ ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _un
 }        //namespace wolf
 
 // Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline)
 } // namespace wolf
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
index faeb7d112..7745e65b1 100644
--- a/src/sensor/sensor_GPS.cpp
+++ b/src/sensor/sensor_GPS.cpp
@@ -1,7 +1,7 @@
 
-#include "sensor_GPS.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/sensor/sensor_GPS.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 
 namespace wolf {
 
@@ -34,7 +34,6 @@ StateBlockPtr SensorGPS::getMapOPtr() const
     return getStateBlockPtrStatic(4);
 }
 
-
 // Define the factory method
 SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
                               const IntrinsicsBasePtr _intrinsics)
@@ -50,9 +49,8 @@ SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::Ve
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("GPS",SensorGPS)
 } // namespace wolf
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index fa1f46eb1..788dc06dc 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/sensor_GPS_fix.cpp
@@ -1,6 +1,6 @@
-#include "sensor_GPS_fix.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/sensor/sensor_GPS_fix.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 
 namespace wolf {
 
@@ -22,7 +22,6 @@ SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFix
     //
 }
 
-
 SensorGPSFix::~SensorGPSFix()
 {
     //
@@ -42,9 +41,8 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix)
 } // namespace wolf
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index 9e5da5f0d..94a30bb43 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -1,6 +1,6 @@
-#include "sensor_IMU.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 
 namespace wolf {
 
@@ -40,7 +40,6 @@ SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _p
     assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
 }
 
-
 SensorIMU::~SensorIMU()
 {
     //
@@ -61,9 +60,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("IMU", SensorIMU)
 } // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 429aaf9f1..a05fec1ce 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,6 +1,5 @@
-#include "sensor_base.h"
-#include "state_block.h"
-
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
 
 namespace wolf {
 
@@ -103,8 +102,6 @@ void SensorBase::removeStateBlocks()
     }
 }
 
-
-
 void SensorBase::fix()
 {
     for( auto sbp : state_block_vec_)
@@ -165,8 +162,6 @@ void SensorBase::unfixIntrinsics()
     updateCalibSize();
 }
 
-
-
 void SensorBase::registerNewStateBlocks()
 {
     if (getProblem() != nullptr)
@@ -283,7 +278,6 @@ SizeEigen SensorBase::computeCalibSize() const
     return sz;
 }
 
-
 Eigen::VectorXs SensorBase::getCalibration() const
 {
     SizeEigen index = 0;
@@ -301,7 +295,6 @@ Eigen::VectorXs SensorBase::getCalibration() const
     return calib;
 }
 
-
 bool SensorBase::process(const CaptureBasePtr capture_ptr)
 {
     capture_ptr->setSensorPtr(shared_from_this());
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 7a9a9a2c7..3a3dae9bd 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -1,8 +1,8 @@
-#include "sensor_camera.h"
+#include "base/sensor/sensor_camera.h"
 
-#include "pinhole_tools.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/pinhole_tools.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 
 namespace wolf
 {
@@ -51,7 +51,6 @@ Eigen::Matrix3s SensorCamera::setIntrinsicMatrix(Eigen::Vector4s _pinhole_model)
     return K;
 }
 
-
 // Define the factory method
 SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
                                  const Eigen::VectorXs& _extrinsics_pq, //
@@ -66,12 +65,11 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
     return sen_ptr;
 }
 
-
 } // namespace wolf
 
 // Register in the SensorFactory
-#include "sensor_factory.h"
-//#include "factory.h"
+#include "base/sensor/sensor_factory.h"
+//#include "base/factory.h"
 namespace wolf
 {
 WOLF_REGISTER_SENSOR("CAMERA", SensorCamera)
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 4c57144c9..d710d9e77 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -1,7 +1,7 @@
-#include "sensor_diff_drive.h"
-#include "../state_block.h"
-#include "../capture_motion.h"
-#include "../eigen_assert.h"
+#include "base/sensor/sensor_diff_drive.h"
+#include "base/state_block.h"
+#include "base/capture/capture_motion.h"
+#include "base/eigen_assert.h"
 
 namespace wolf {
 
@@ -46,7 +46,6 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
   return odo;
 }
 
-
 /// @todo Further work to enforce wheel model
 
 //const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics()
@@ -127,9 +126,8 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
 } // namespace wolf
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index 227734568..a7a5677af 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -1,5 +1,5 @@
-#include "sensor_laser_2D.h"
-#include "state_block.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/state_block.h"
 
 namespace wolf {
 
@@ -79,14 +79,10 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen
     return sen;
 }
 
-
 } // namespace wolf
 
-
-
-
 // Register in the SensorFactory and the ParameterFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 //#include "intrinsics_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index fdb7ab480..7f0abaed4 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
-#include "sensor_odom_2D.h"
-#include "state_block.h"
-#include "state_angle.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/state_block.h"
+#include "base/state_angle.h"
 
 namespace wolf {
 
@@ -27,7 +27,6 @@ SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _int
     //
 }
 
-
 SensorOdom2D::~SensorOdom2D()
 {
     //
@@ -69,9 +68,8 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D)
 } // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index c5f3fb388..aa6efc42b 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3D.cpp
@@ -5,10 +5,10 @@
  *      \author: jsola
  */
 
-#include "sensor_odom_3D.h"
+#include "base/sensor/sensor_odom_3D.h"
 
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 
 namespace wolf {
 
@@ -44,7 +44,6 @@ SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom
     //
 }
 
-
 SensorOdom3D::~SensorOdom3D()
 {
     //
@@ -69,9 +68,8 @@ SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen:
 
 } // namespace wolf
 
-
 // Register in the SensorFactory
-#include "sensor_factory.h"
+#include "base/sensor/sensor_factory.h"
 namespace wolf {
 WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D)
 }
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index b6a2e6a40..f85416468 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -1,7 +1,7 @@
-#include "solver_manager.h"
-#include "../trajectory_base.h"
-#include "../map_base.h"
-#include "../landmark_base.h"
+#include "base/solver/solver_manager.h"
+#include "base/trajectory_base.h"
+#include "base/map_base.h"
+#include "base/landmark/landmark_base.h"
 
 namespace wolf {
 
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index cb39335b0..a25c72bef 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -1,4 +1,4 @@
-#include "ceres_manager.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 SolverManager::SolverManager()
 {
diff --git a/src/state_block.cpp b/src/state_block.cpp
index 2599ea21c..46411e7c3 100644
--- a/src/state_block.cpp
+++ b/src/state_block.cpp
@@ -1,4 +1,4 @@
-#include "state_block.h"
+#include "base/state_block.h"
 namespace wolf
 {
 
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index 543ceb2ca..55d25fc6e 100644
--- a/src/time_stamp.cpp
+++ b/src/time_stamp.cpp
@@ -1,5 +1,5 @@
 
-#include "time_stamp.h"
+#include "base/time_stamp.h"
 
 namespace wolf {
 
@@ -10,8 +10,6 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
     return os;
 }
 
-
-
 TimeStamp::TimeStamp() :
         //time_stamp_(0)
         time_stamp_nano_(0)
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
index e51b38614..8bb0e0abe 100644
--- a/src/track_matrix.cpp
+++ b/src/track_matrix.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "track_matrix.h"
+#include "base/track_matrix.h"
 
 namespace wolf
 {
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 3dcb41267..557d072fd 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -1,6 +1,5 @@
-#include "trajectory_base.h"
-#include "frame_base.h"
-
+#include "base/trajectory_base.h"
+#include "base/frame_base.h"
 
 namespace wolf {
 
@@ -39,7 +38,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
     return _frame_ptr;
 }
 
-
 void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
 {
 	for(auto fr_ptr : getFrameList())
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 0e6587e29..38b83b82f 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -6,11 +6,11 @@
  */
 
 // wolf yaml
-#include "processor_IMU.h"
-#include "yaml_conversion.h"
+#include "base/processor/processor_IMU.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../factory.h"
+#include "base/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 7de4b1587..051174ef5 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -6,15 +6,15 @@
  */
 
 // wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../factory.h"
+#include "base/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
 
-#include "../processor_params_image.h"
+#include "base/processor/processor_params_image.h"
 
 namespace wolf
 {
@@ -64,7 +64,5 @@ static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _fi
 const bool WOLF_UNUSED registered_prc_image_feature_par = ProcessorParamsFactory::get().registerCreator("IMAGE FEATURE", createProcessorParamsImage);
 const bool WOLF_UNUSED registered_prc_image_landmark_par = ProcessorParamsFactory::get().registerCreator("IMAGE LANDMARK", createProcessorParamsImage);
 
-
-}
 }
 
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index 1b5a3f40d..af8934b07 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -6,11 +6,11 @@
  */
 
 // wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../processor_odom_3D.h"
-#include "../factory.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/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 9e339d41a..2a6ae97f7 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -5,13 +5,12 @@
  *      Author: asantamaria
  */
 
-
 // wolf yaml
-#include "../processors/processor_tracker_feature_trifocal.h"
-#include "yaml_conversion.h"
+#include "processor/processor_tracker_feature_trifocal.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../factory.h"
+#include "base/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
@@ -58,7 +57,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
         params->max_euclidean_distance          = algorithm["max euclidean distance"]         .as<Scalar>();
         params->min_track_length_for_constraint = algorithm["min track length for constraint"].as<int>();
 
-
         YAML::Node noise                      = config["noise"];
         params->pixel_noise_std               = noise ["pixel noise std"].as<Scalar>();
 
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index 658becb6b..c64914c60 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -6,11 +6,11 @@
  */
 
 // wolf yaml
-#include "sensor_IMU.h"
-#include "yaml_conversion.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../factory.h"
+#include "base/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 852adc977..d1a76b78b 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -6,11 +6,11 @@
  */
 
 // wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../sensor_camera.h"
-#include "../factory.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
@@ -59,7 +59,6 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
         intrinsics_cam->width = width;
         intrinsics_cam->height = height;
 
-
         //=========================================
         // ===== this part for debugging only =====
         //=========================================
@@ -80,9 +79,6 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
 //        std::cout << "\tintrinsic   : " << intrinsic.transpose() << std::endl;
 //        std::cout << "\tdistoriton  : " << distortion.transpose() << std::endl;
         //=========================================
-        //=========================================
-
-
 
         return intrinsics_cam;
     }
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index 160386e6e..6a6b3f4ae 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -5,14 +5,13 @@
  *      \author: jsola
  */
 
-
 // wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-//#include "../intrinsics_factory.h"
-#include "../factory.h"
-#include "../sensor_laser_2D.h"
+//#include "base/intrinsics_factory.h"
+#include "base/factory.h"
+#include "base/sensor/sensor_laser_2D.h"
 
 // yaml library
 #include <yaml-cpp/yaml.h>
@@ -29,7 +28,6 @@ IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
     return params;
 }
 
-
 // register into factory
 const bool WOLF_UNUSED registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
 
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index a6012aeef..f423169a9 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -6,11 +6,11 @@
  */
 
 // wolf yaml
-#include "yaml_conversion.h"
+#include "base/yaml/yaml_conversion.h"
 
 // wolf
-#include "../sensor_odom_3D.h"
-#include "../factory.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/factory.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index c423f80a7..e9261f75b 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -6,15 +6,15 @@
  */
 
 //Wolf
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "wolf.h"
-#include "sensor_odom_3D.h"
-#include "processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/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 "logging.h"
+#include "base/logging.h"
 
 // make my life easier
 using namespace Eigen;
@@ -24,7 +24,6 @@ using std::make_shared;
 using std::static_pointer_cast;
 using std::string;
 
-
 class Process_Constraint_IMU : public testing::Test
 {
     public:
@@ -83,7 +82,6 @@ class Process_Constraint_IMU : public testing::Test
         bool                p0_fixed, q0_fixed, v0_fixed;
         bool                p1_fixed, q1_fixed, v1_fixed;
 
-
         virtual void SetUp( )
         {
             string wolf_root = _WOLF_ROOT_DIR;
@@ -114,7 +112,6 @@ class Process_Constraint_IMU : public testing::Test
 
         }
 
-
         
         /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
          * Input:
@@ -153,7 +150,6 @@ class Process_Constraint_IMU : public testing::Test
             Delta               = Delta_plus;
         }
 
-
         /* Integrate acc and angVel motion, obtain Delta_preintegrated
          * Input:
          *   N: number of steps
@@ -258,8 +254,6 @@ class Process_Constraint_IMU : public testing::Test
             return trajectory;
         }
 
-
-
         MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected)
         {
             Vector6s      motion_now;
@@ -288,8 +282,6 @@ class Process_Constraint_IMU : public testing::Test
             return processor_imu->getBuffer();
         }
 
-
-
         // Initial configuration of variables
         bool configureAll()
         {
@@ -326,8 +318,6 @@ class Process_Constraint_IMU : public testing::Test
             return true;
         }
 
-
-
         // Integrate using all methods
         virtual void integrateAll()
         {
@@ -338,7 +328,6 @@ class Process_Constraint_IMU : public testing::Test
                 D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
             x1_exact = imu::composeOverState(x0, D_exact, DT );
 
-
             // ===================================== INTEGRATE USING IMU_TOOLS
             // pre-integrate
             if (motion.cols() == 1)
@@ -363,7 +352,6 @@ class Process_Constraint_IMU : public testing::Test
             x1_corrected     = imu::composeOverState(x0, D_corrected     , DT );
         }
 
-
         // Integrate Trajectories all methods
         virtual void integrateAllTrajectories()
         {
@@ -371,7 +359,6 @@ class Process_Constraint_IMU : public testing::Test
             Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
             Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
 
-
             // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
             MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
 
@@ -425,7 +412,6 @@ class Process_Constraint_IMU : public testing::Test
 
             MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
 
-
             // Build trajectories preintegrated and corrected with processorIMU
             Dt = 0;
             col = 0;
@@ -451,8 +437,6 @@ class Process_Constraint_IMU : public testing::Test
             x1_corrected     = imu::composeOverState(x0, D_corrected     , DT );
         }
 
-
-
         // Set state_blocks status
         void setFixedBlocks()
         {
@@ -465,8 +449,6 @@ class Process_Constraint_IMU : public testing::Test
             KF_1->getVPtr()->setFixed(v1_fixed);
         }
 
-
-
         void setKfStates()
         {
             // This perturbs states to estimate around the exact value, then assigns to the keyframe
@@ -495,8 +477,6 @@ class Process_Constraint_IMU : public testing::Test
             KF_1->setState(x_pert);
         }
 
-
-
         virtual void buildProblem()
         {
             // ===================================== SET KF in Wolf tree
@@ -519,8 +499,6 @@ class Process_Constraint_IMU : public testing::Test
             setKfStates();
         }
 
-
-
         string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
         {
             string report   = ceres_manager->solve(verbose);
@@ -542,8 +520,6 @@ class Process_Constraint_IMU : public testing::Test
             return report;
         }
 
-
-
         string runAll(SolverManager::ReportVerbosity verbose)
         {
             configureAll();
@@ -553,8 +529,6 @@ class Process_Constraint_IMU : public testing::Test
             return report;
         }
 
-
-
         void printAll(std::string report = "")
         {
             WOLF_TRACE(report);
@@ -585,8 +559,6 @@ class Process_Constraint_IMU : public testing::Test
             WOLF_TRACE("X0_optim      : ", x0_optim           .transpose() ); // optimal state after solving using processor
         }
 
-
-
         virtual void assertAll()
         {
             // check delta and state integrals
@@ -729,7 +701,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -774,7 +745,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001,   .003, -.002, .001).finished());
     sensor_imu->getIntrinsicPtr()->setState(initial_bias);
@@ -786,7 +756,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
     ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
 }
 
-
 TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
 {
 
@@ -822,7 +791,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -832,7 +800,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
 
 }
 
-
 TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
 {
 
@@ -868,7 +835,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -878,7 +844,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
 
 }
 
-
 TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
 {
 
@@ -914,7 +879,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -924,7 +888,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
 
 }
 
-
 TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
 {
 
@@ -960,7 +923,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1005,7 +967,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1050,7 +1011,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1095,7 +1055,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1140,7 +1099,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1185,7 +1143,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1230,7 +1187,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1275,7 +1231,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1320,7 +1275,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1365,7 +1319,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1410,7 +1363,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1455,7 +1407,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     string report = runAll(SolverManager::ReportVerbosity::BRIEF);
 
@@ -1500,7 +1451,6 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
     // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
     // ================================================================================================================ //
 
-
     // ===================================== RUN ALL
     configureAll();
     integrateAllTrajectories();
@@ -1561,8 +1511,6 @@ int main(int argc, char **argv)
     return RUN_ALL_TESTS();
 }
 
-
-
 /* Some notes :
  *
  * - Process_Constraint_IMU_ODO.MotionConstant_PQv_b__PQv_b :
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
index 9f0f8c398..e41007154 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_IMU_tools.cpp
@@ -5,10 +5,9 @@
  *      Author: jsola
  */
 
-#include "IMU_tools.h"
+#include "base/IMU_tools.h"
 #include "utils_gtest.h"
 
-
 using namespace Eigen;
 using namespace wolf;
 using namespace imu;
@@ -376,7 +375,6 @@ TEST(motion2data, AllRandom)
     Vector6s bias;
     Quaternions q;
 
-
     motion      .setRandom();
     bias        .setRandom();
     q.coeffs()  .setRandom().normalize();
@@ -390,7 +388,6 @@ TEST(motion2data, AllRandom)
     ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
 }
 
-
 /* Integrate acc and angVel motion, obtain Delta_preintegrated
  * Input:
  *   N: number of steps
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 00fdf8883..d1ec1884c 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -5,11 +5,10 @@
  *      Author: jsola
  */
 
-
 #include "utils_gtest.h"
 
-#include "capture_base.h"
-#include "state_angle.h"
+#include "base/capture/capture_base.h"
+#include "base/state_angle.h"
 
 using namespace wolf;
 using namespace Eigen;
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 0df86c75e..097969799 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -6,18 +6,18 @@
  */
 
 #include "utils_gtest.h"
-#include "../src/logging.h"
-
-#include "../problem.h"
-#include "../sensor_base.h"
-#include "../state_block.h"
-#include "../capture_void.h"
-#include "../constraint_pose_2D.h"
-#include "../constraint_quaternion_absolute.h"
-#include "../solver/solver_manager.h"
-#include "../ceres_wrapper/ceres_manager.h"
-#include "../local_parametrization_angle.h"
-#include "../local_parametrization_quaternion.h"
+#include "base/logging.h"
+
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/capture/capture_void.h"
+#include "base/constraint/constraint_pose_2D.h"
+#include "base/constraint/constraint_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 "ceres/ceres.h"
 
@@ -629,7 +629,6 @@ TEST(CeresManager, ConstraintsUpdateLocalParam)
     ceres_manager_ptr->check();
 }
 
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_constraint_IMU.cpp b/test/gtest_constraint_IMU.cpp
index bc212aaaa..7ef93e9a3 100644
--- a/test/gtest_constraint_IMU.cpp
+++ b/test/gtest_constraint_IMU.cpp
@@ -6,15 +6,15 @@
  */
 
 //Wolf
-#include "capture_IMU.h"
-#include "constraint_pose_3D.h"
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "processor_odom_3D.h"
+#include "base/capture/capture_IMU.h"
+#include "base/constraint/constraint_pose_3D.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/processor/processor_odom_3D.h"
 #include "ceres_wrapper/ceres_manager.h"
 
 #include "utils_gtest.h"
-#include "logging.h"
+#include "base/logging.h"
 
 #include <iostream>
 #include <fstream>
@@ -90,7 +90,6 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
 
         //===================================================== END{INITIALIZATION}
 
-
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
@@ -436,7 +435,6 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
 
         //===================================================== END{INITIALIZATION}
 
-
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
@@ -1011,7 +1009,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
 //        WOLF_TRACE("KF2 delta cor: ", last_CM  ->getDeltaCorrected(origin_CB->getCalibration()).transpose());
 //        WOLF_TRACE("KF2 jacob    : ", last_CM  ->getJacobianCalib().row(0));
 
-
         // ==================================================== show problem status
 
         problem->print(4,1,1,1);
@@ -1335,8 +1332,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
     virtual void TearDown(){}
 };
 
-
-
 // tests with following conditions :
 //  var(b1,b2),        invar(p1,q1,v1,p2,q2,v2),    factor : imu(p,q,v)
 
@@ -1642,7 +1637,6 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
 
-
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
     epsilon_bias = 0.01;
@@ -1723,7 +1717,6 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
 
-
     //==============================================================
     //WOLF_INFO("Starting error bias 1e-2")
     epsilon_bias = 0.01;
@@ -2843,6 +2836,5 @@ int main(int argc, char **argv)
 //  ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
 //  ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
 
-
   return RUN_ALL_TESTS();
 }
diff --git a/test/gtest_constraint_absolute.cpp b/test/gtest_constraint_absolute.cpp
index a0587e29c..53252fc5f 100644
--- a/test/gtest_constraint_absolute.cpp
+++ b/test/gtest_constraint_absolute.cpp
@@ -5,14 +5,12 @@
  *      \author: datchuth
  */
 
-
 #include "utils_gtest.h"
-#include "constraint_block_absolute.h"
-#include "constraint_quaternion_absolute.h"
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/constraint/constraint_block_absolute.h"
+#include "base/constraint/constraint_quaternion_absolute.h"
+#include "base/capture/capture_motion.h"
 
+#include "base/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
diff --git a/test/gtest_constraint_autodiff.cpp b/test/gtest_constraint_autodiff.cpp
index c5820537f..5138bb899 100644
--- a/test/gtest_constraint_autodiff.cpp
+++ b/test/gtest_constraint_autodiff.cpp
@@ -5,15 +5,14 @@
  *      Author: jvallve
  */
 
-
 #include "utils_gtest.h"
 
-#include "sensor_odom_2D.h"
-#include "capture_void.h"
-#include "feature_odom_2D.h"
-#include "constraint_odom_2D.h"
-#include "constraint_odom_2D_analytic.h"
-#include "constraint_autodiff.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/capture/capture_void.h"
+#include "base/feature/feature_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/constraint/constraint_odom_2D_analytic.h"
+#include "base/constraint/constraint_autodiff.h"
 
 using namespace wolf;
 using namespace Eigen;
diff --git a/test/gtest_constraint_autodiff_distance_3D.cpp b/test/gtest_constraint_autodiff_distance_3D.cpp
index bc6e402e4..ae3313afe 100644
--- a/test/gtest_constraint_autodiff_distance_3D.cpp
+++ b/test/gtest_constraint_autodiff_distance_3D.cpp
@@ -5,11 +5,11 @@
  *      \author: jsola
  */
 
-#include "constraints/constraint_autodiff_distance_3D.h"
-#include "problem.h"
-#include "logging.h"
-#include "ceres_wrapper/ceres_manager.h"
-#include "rotations.h"
+#include "base/constraint/constraint_autodiff_distance_3D.h"
+#include "base/problem.h"
+#include "base/logging.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/rotations.h"
 
 #include "utils_gtest.h"
 
diff --git a/test/gtest_constraint_autodiff_trifocal.cpp b/test/gtest_constraint_autodiff_trifocal.cpp
index aa34b7ea7..1615cc4ab 100644
--- a/test/gtest_constraint_autodiff_trifocal.cpp
+++ b/test/gtest_constraint_autodiff_trifocal.cpp
@@ -1,10 +1,10 @@
 #include "utils_gtest.h"
 
-#include "logging.h"
+#include "base/logging.h"
 
 #include "ceres_wrapper/ceres_manager.h"
 #include "processors/processor_tracker_feature_trifocal.h"
-#include "capture_image.h"
+#include "base/capture/capture_image.h"
 #include "constraints/constraint_autodiff_trifocal.h"
 
 using namespace Eigen;
@@ -278,7 +278,6 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
 
     ASSERT_MATRIX_APPROX(J_r_m1, Jn_r_m1, 1e-6);
 
-
     // jacs wrt m2
     pix0 = c123->getPixelCanonicalOrigin();
     for (int i=0; i<3; i++)
@@ -295,7 +294,6 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
 
     ASSERT_MATRIX_APPROX(J_r_m2, Jn_r_m2, 1e-6);
 
-
     // jacs wrt m3
     pix0 = c123->getPixelCanonicalLast();
     for (int i=0; i<3; i++)
@@ -671,7 +669,6 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc
          *
          */
 
-
     public:
         std::vector<FeatureBasePtr> fv1, fv2, fv3;
         std::vector<ConstraintAutodiffTrifocalPtr> cv123;
@@ -735,7 +732,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
      *
      */
 
-
     S ->getPPtr()->fix(); // do not calibrate sensor pos
     S ->getOPtr()->fix(); // do not calibrate sensor ori
     F1->getPPtr()->fix(); // Cam 1 acts as reference
@@ -801,7 +797,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
      *
      */
 
-
     S ->getPPtr()->fix(); // do not calibrate sensor pos
     S ->getOPtr()->fix(); // do not calibrate sensor ori
     F1->getPPtr()->fix(); // Cam 1 acts as reference
@@ -868,7 +863,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
      *
      */
 
-
     S ->getPPtr()->fix(); // do not calibrate sensor pos
     S ->getOPtr()->fix(); // do not calibrate sensor ori
     F1->getPPtr()->fix(); // Cam 1 acts as reference
diff --git a/test/gtest_constraint_odom_3D.cpp b/test/gtest_constraint_odom_3D.cpp
index 684082775..d71050ec6 100644
--- a/test/gtest_constraint_odom_3D.cpp
+++ b/test/gtest_constraint_odom_3D.cpp
@@ -5,14 +5,12 @@
  *      \author: jsola
  */
 
-
 #include "utils_gtest.h"
 
-#include "constraint_odom_3D.h"
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/constraint/constraint_odom_3D.h"
+#include "base/capture/capture_motion.h"
 
+#include "base/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
diff --git a/test/gtest_constraint_pose_2D.cpp b/test/gtest_constraint_pose_2D.cpp
index dd95c82d8..125893e67 100644
--- a/test/gtest_constraint_pose_2D.cpp
+++ b/test/gtest_constraint_pose_2D.cpp
@@ -5,14 +5,12 @@
  *      \author: jsola
  */
 
-
-#include "../constraint_pose_2D.h"
+#include "base/constraint/constraint_pose_2D.h"
 #include "utils_gtest.h"
 
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_motion.h"
 
+#include "base/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -65,7 +63,6 @@ TEST(ConstraintPose2D, solve)
 
 }
 
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_constraint_pose_3D.cpp b/test/gtest_constraint_pose_3D.cpp
index 48fbedebd..05de4bb61 100644
--- a/test/gtest_constraint_pose_3D.cpp
+++ b/test/gtest_constraint_pose_3D.cpp
@@ -5,14 +5,12 @@
  *      \author: jsola
  */
 
-
-#include "../constraint_pose_3D.h"
+#include "base/constraint/constraint_pose_3D.h"
 #include "utils_gtest.h"
 
-#include "capture_motion.h"
-
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/capture/capture_motion.h"
 
+#include "base/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -24,7 +22,6 @@ Vector7s pose6toPose7(Vector6s _pose6)
     return (Vector7s() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished();
 }
 
-
 // Input pose6 and covariance
 Vector6s pose(Vector6s::Random());
 Vector7s pose7 = pose6toPose7(pose);
@@ -46,7 +43,6 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D"
 FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
 ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0)));
 
-
 ////////////////////////////////////////////////////////
 
 TEST(ConstraintPose3D, check_tree)
@@ -73,7 +69,6 @@ TEST(ConstraintPose3D, solve)
 
 }
 
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_constraint_sparse.cpp b/test/gtest_constraint_sparse.cpp
index 875c68c8e..900f39d5b 100644
--- a/test/gtest_constraint_sparse.cpp
+++ b/test/gtest_constraint_sparse.cpp
@@ -5,7 +5,6 @@
  *      \author: jsola
  */
 
-
 #include "utils_gtest.h"
 
 #include "constraint_sparse.h"
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index 23fa8e234..649ba85e9 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,6 +1,6 @@
 #include "utils_gtest.h"
 
-#include "../eigen_predicates.h"
+#include "base/eigen_predicates.h"
 
 TEST(TestEigenPredicates, TestEigenDynPredZero)
 {
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 970556a3e..5ce136d66 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -1,13 +1,13 @@
 //Wolf
-#include "capture_IMU.h"
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#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 "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
 
 class FeatureIMU_test : public testing::Test
 {
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index bc867f48f..f9da19cff 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -5,11 +5,10 @@
  *      Author: jsola
  */
 
-#include "feature_base.h"
+#include "base/feature/feature_base.h"
 
 #include "utils_gtest.h"
 
-
 using namespace wolf;
 using namespace Eigen;
 
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index ae6b78068..e5a4d4f3c 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,20 +5,17 @@
  *      Author: jsola
  */
 
-
-
 #include "utils_gtest.h"
-#include "../logging.h"
+#include "base/logging.h"
 
-#include "../frame_base.h"
-#include "../sensor_odom_2D.h"
-#include "../processor_odom_2D.h"
-#include "../constraint_odom_2D.h"
-#include "../capture_motion.h"
+#include "base/frame_base.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
+#include "base/capture/capture_motion.h"
 
 #include <iostream>
 
-
 using namespace Eigen;
 using namespace std;
 using namespace wolf;
@@ -61,7 +58,6 @@ TEST(FrameBase, LinksBasic)
     ASSERT_EQ(F->getHits() , (unsigned int) 0);
 }
 
-
 TEST(FrameBase, LinksToTree)
 {
     // Problem with 2 frames and one motion constraint between them
@@ -120,8 +116,6 @@ TEST(FrameBase, LinksToTree)
     F1->getOPtr()->fix();
     ASSERT_TRUE(F1->isFixed());
 
-
-
     // set key
     F1->setKey();
     ASSERT_TRUE(F1->isKey());
@@ -131,7 +125,7 @@ TEST(FrameBase, LinksToTree)
     ASSERT_TRUE(F1->getCaptureList().empty());
 }
 
-#include "state_quaternion.h"
+#include "base/state_quaternion.h"
 TEST(FrameBase, GetSetState)
 {
     // Create PQV_3D state blocks
@@ -166,13 +160,9 @@ TEST(FrameBase, GetSetState)
     ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL));
 }
 
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
 
-
-
-
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index bdd413a82..42e01489e 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -6,14 +6,13 @@
  */
 
 #include "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
 
+#include "base/local_parametrization_quaternion.h"
+#include "base/local_parametrization_homogeneous.h"
+#include "base/rotations.h"
 
-#include "../src/local_parametrization_quaternion.h"
-#include "../src/local_parametrization_homogeneous.h"
-#include "../src/rotations.h"
-
-#include "../src/wolf.h"
+#include "base/wolf.h"
 
 #include <iostream>
 
@@ -32,7 +31,6 @@
     } \
 }
 
-
 using namespace Eigen;
 using namespace std;
 using namespace wolf;
@@ -126,7 +124,6 @@ TEST(TestLocalParametrization, QuaternionGlobal)
 
     ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10);
 
-
 }
 
 TEST(TestLocalParametrization, Homogeneous)
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index d2b82d1ea..1dfcce935 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "wolf.h"
+#include "base/wolf.h"
 
 using namespace Eigen;
 using namespace wolf;
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 3bed94433..6d0301692 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -5,13 +5,12 @@
  *      Author: jsola
  */
 
-
 #include "utils_gtest.h"
-#include "../logging.h"
+#include "base/logging.h"
 
-#include "../motion_buffer.h"
+#include "base/motion_buffer.h"
 
-#include "wolf.h"
+#include "base/wolf.h"
 
 #include <iostream>
 
@@ -184,4 +183,3 @@ int main(int argc, char **argv)
   return RUN_ALL_TESTS();
 }
 
-
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index d8e10b949..fcf60798e 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -8,15 +8,14 @@
 #include "utils_gtest.h"
 
 // Classes under test
-#include "../processor_odom_2D.h"
-#include "../constraint_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/constraint/constraint_odom_2D.h"
 
 // Wolf includes
-#include "../sensor_odom_2D.h"
-#include "../state_block.h"
-#include "../wolf.h"
-#include "../ceres_wrapper/ceres_manager.h"
-#include "../capture_pose.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/state_block.h"
+#include "base/wolf.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 // STL includes
 #include <map>
@@ -27,11 +26,11 @@
 // General includes
 #include <iostream>
 #include <iomanip>      // std::setprecision
+#include "base/capture/capture_pose.h"
 
 using namespace wolf;
 using namespace Eigen;
 
-
 VectorXs plus(const VectorXs& _pose, const Vector2s& _data)
 {
     VectorXs _pose_plus_data(3);
@@ -98,7 +97,6 @@ void show(const ProblemPtr& problem)
     }
 }
 
-
 TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
 {
     using std::cout;
@@ -131,7 +129,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
 
     // KF1 and motion from KF0
     t += dt;
-    t += dt;
     FrameBasePtr        F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
     CaptureBasePtr      C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
     FeatureBasePtr      f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
@@ -140,7 +137,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
 
     // KF2 and motion from KF1
     t += dt;
-    t += dt;
     FrameBasePtr        F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
     CaptureBasePtr      C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
     FeatureBasePtr      f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
@@ -471,8 +467,6 @@ TEST(Odom2D, KF_callback)
     }
 }
 
-
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index bc36ac845..864f19c14 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -5,19 +5,15 @@
  *      \author: jsola
  */
 
-
-
-
 #include "utils_gtest.h"
 
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
 
-#include "processor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
 
 #include <iostream>
 
-
 #define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
 {   VectorXs Do(7); \
     prc_ptr->deltaPlusDelta(D, d, dt, Do); \
@@ -155,7 +151,6 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
 
 }
 
-
 TEST(ProcessorOdom3D, Interpolate0) // basic test
 {
     /* Conditions:
@@ -177,7 +172,6 @@ TEST(ProcessorOdom3D, Interpolate0) // basic test
     WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
     WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
 
-
     // set final
     final.ts_ = 5;
     final.delta_        << 1,0,0, 0,0,0,1;
@@ -271,7 +265,6 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
     // Motion structures
     Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
 
-
     /////////// start experiment ///////////////
 
     // set origin and ref states
@@ -315,7 +308,6 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
     Dp_of = q_o.conjugate() * (p_f - p_o);
     Dq_of = q_o.conjugate() *  q_f;
 
-
     // set ref
     R.ts_           = t_r;
     R.delta_        = dx_or; // origin to ref
@@ -405,7 +397,6 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
     // Motion structures
     Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
 
-
     /////////// start experiment ///////////////
 
     // set final and ref deltas
@@ -495,13 +486,9 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
 
 }
 
-
-
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
 
-
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 1204254c1..8f68a5bd5 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -7,13 +7,13 @@
 //Wolf
 #include "utils_gtest.h"
 
-#include "processor_odom_2D.h"
-#include "sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
 
-#include "processor_tracker_feature_dummy.h"
-#include "capture_void.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
 
-#include "problem.h"
+#include "base/problem.h"
 
 // STL
 #include <iterator>
diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp
index b46e682ab..fb36c6d12 100644
--- a/test/gtest_pinhole.cpp
+++ b/test/gtest_pinhole.cpp
@@ -5,10 +5,9 @@
  *      Author: jsola
  */
 
-#include "pinhole_tools.h"
+#include "base/pinhole_tools.h"
 #include "utils_gtest.h"
 
-
 using namespace Eigen;
 using namespace wolf;
 using namespace pinhole;
@@ -200,4 +199,3 @@ int main(int argc, char **argv)
   return RUN_ALL_TESTS();
 }
 
-
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 7ae62578b..1cf49acaa 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -6,13 +6,13 @@
  */
 
 #include "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
 
-#include "../problem.h"
-#include "../sensor_base.h"
-#include "../sensor_odom_3D.h"
-#include "../processor_odom_3D.h"
-#include "../processor_tracker_feature_dummy.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/sensor/sensor_odom_3D.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
 
 #include <iostream>
 
@@ -127,7 +127,6 @@ TEST(Problem, SetOrigin_PO_2D)
     // check that the constraint is absolute (no pointers to other F, f, or L)
     ConstraintBasePtr c = f->getConstraintList().front();
     ASSERT_FALSE(c->getFrameOtherPtr());
-    ASSERT_FALSE(c->getFrameOtherPtr());
     ASSERT_FALSE(c->getLandmarkOtherPtr());
 
     // check that the Feature measurement and covariance are the ones provided
@@ -137,7 +136,6 @@ TEST(Problem, SetOrigin_PO_2D)
     //    P->print(4,1,1,1);
 }
 
-
 TEST(Problem, SetOrigin_PO_3D)
 {
     ProblemPtr P = Problem::create("PO 3D");
@@ -166,7 +164,6 @@ TEST(Problem, SetOrigin_PO_3D)
     // check that the constraint is absolute (no pointers to other F, f, or L)
     ConstraintBasePtr c = f->getConstraintList().front();
     ASSERT_FALSE(c->getFrameOtherPtr());
-    ASSERT_FALSE(c->getFrameOtherPtr());
     ASSERT_FALSE(c->getLandmarkOtherPtr());
 
     // check that the Feature measurement and covariance are the ones provided
@@ -176,8 +173,6 @@ TEST(Problem, SetOrigin_PO_3D)
     //    P->print(4,1,1,1);
 }
 
-
-
 TEST(Problem, emplaceFrame_factory)
 {
     ProblemPtr P = Problem::create("PO 2D");
@@ -203,7 +198,6 @@ TEST(Problem, emplaceFrame_factory)
     ASSERT_EQ(f2->getProblem(), P);
 }
 
-
 TEST(Problem, StateBlocks)
 {
     std::string wolf_root = _WOLF_ROOT_DIR;
@@ -235,7 +229,6 @@ TEST(Problem, StateBlocks)
     ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2));
     ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
 
-
     //    P->print(4,1,1,1);
 
     // change some SB properties
@@ -277,7 +270,6 @@ TEST(Problem, Covariances)
 
 }
 
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 8dfb9bd57..a6ee8c8ac 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -5,16 +5,16 @@
  *      \author: jsola
  */
 
-#include "capture_IMU.h"
-#include "processor_IMU.h"
-#include "sensor_IMU.h"
-#include "wolf.h"
+#include "base/capture/capture_IMU.h"
+#include "base/processor/processor_IMU.h"
+#include "base/sensor/sensor_IMU.h"
+#include "base/wolf.h"
 
 #include "utils_gtest.h"
-#include "logging.h"
+#include "base/logging.h"
 
-#include "rotations.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/rotations.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 #include <cmath>
 #include <iostream>
@@ -78,12 +78,10 @@ class ProcessorIMUt : public testing::Test
     }
 };
 
-
 TEST(ProcessorIMU_constructors, ALL)
 {
     using namespace wolf;
 
-
     //constructor with ProcessorIMUParamsPtr argument only
     ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
     param_ptr->max_time_span =   2.0;
@@ -248,7 +246,6 @@ ASSERT_MATRIX_APPROX(mot_int.data_,  mot_int_gt.data_, 1e-6);
 //ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
 ASSERT_MATRIX_APPROX(mot_final.delta_integr_,  mot_sec.delta_integr_, 1e-6);
 
-
 }
 
 TEST_F(ProcessorIMUt, acc_x)
@@ -351,7 +348,6 @@ TEST_F(ProcessorIMUt, acc_z)
     ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
 }
 
-
 TEST_F(ProcessorIMUt, check_Covariance)
 {
     t.set(0); // clock in 0,1 ms ticks
@@ -394,7 +390,6 @@ TEST_F(ProcessorIMUt, gyro_x)
     VectorXs x(10);
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
-
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
 
     //do so for 5s
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
index dc9ab0397..1e3b0940c 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_IMU_jacobians.cpp
@@ -6,13 +6,13 @@
  */
 
  //Wolf
-#include "capture_IMU.h"
-#include "sensor_IMU.h"
+#include "base/capture/capture_IMU.h"
+#include "base/sensor/sensor_IMU.h"
 #include "test/processor_IMU_UnitTester.h"
-#include "wolf.h"
-#include "problem.h"
-#include "state_block.h"
-#include "state_quaternion.h"
+#include "base/wolf.h"
+#include "base/problem.h"
+#include "base/state_block.h"
+#include "base/state_quaternion.h"
 #include <iostream>
 #include <fstream>
 #include <iomanip>
@@ -110,7 +110,6 @@ class ProcessorIMU_jacobians : public testing::Test
     }
 };
 
-
 class ProcessorIMU_jacobians_Dq : public testing::Test
 {
     public:
@@ -470,7 +469,6 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO)
 
 //dDo_dP = dDo_dV = [0, 0, 0]
 
-
 TEST_F(ProcessorIMU_jacobians, dDo_dO)
 {
     using namespace wolf;
@@ -506,7 +504,6 @@ TEST_F(ProcessorIMU_jacobians, dDp_dp)
 
 //dDv_dp = [0, 0, 0]
 
-
 TEST_F(ProcessorIMU_jacobians, dDv_dv)
 {
     using namespace wolf;
@@ -544,7 +541,6 @@ TEST_F(ProcessorIMU_jacobians, dDo_do)
      "\ndDo_do_a - dDo_do_ : \n" << dDo_do_a - dDo_do << std::endl;
 }
 
-
 int main(int argc, char **argv)
 {
     using namespace wolf;
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 23baef7b2..aba232636 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -8,13 +8,13 @@
 //Wolf
 #include "utils_gtest.h"
 
-#include "processor_odom_2D.h"
-#include "sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/sensor/sensor_odom_2D.h"
 
-#include "processor_tracker_feature_dummy.h"
-#include "capture_void.h"
+#include "base/processor/processor_tracker_feature_dummy.h"
+#include "base/capture/capture_void.h"
 
-#include "problem.h"
+#include "base/problem.h"
 
 // STL
 #include <iterator>
@@ -23,7 +23,6 @@
 using namespace wolf;
 using namespace Eigen;
 
-
 TEST(ProcessorBase, KeyFrameCallback)
 {
 
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index dcb32770e..4e2b427fb 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -6,12 +6,11 @@
  *      \author: tessajohanna
  */
 
-
 #include "utils_gtest.h"
-#include "../logging.h"
+#include "base/logging.h"
 
-#include "../sensor_odom_2D.h"
-#include "../processor_frame_nearest_neighbor_filter.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_frame_nearest_neighbor_filter.h"
 
 #include <iostream>
 
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 94a2f0c15..3e379c9a5 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -5,15 +5,14 @@
  *      Author: jsola
  */
 
-
 #include "utils_gtest.h"
 
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
 
-#include "sensor_odom_2D.h"
-#include "processor_odom_2D.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_2D.h"
+#include "base/processor/processor_odom_2D.h"
+#include "base/ceres_wrapper/ceres_manager.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -58,7 +57,6 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
 
 };
 
-
 TEST_F(ProcessorMotion_test, IntegrateStraight)
 {
     data << 1, 0; // advance straight
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 71a8e7f46..47e42c577 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -1,14 +1,14 @@
 #include "utils_gtest.h"
 
-#include "wolf.h"
-#include "logging.h"
+#include "base/wolf.h"
+#include "base/logging.h"
 
 #include "vision_utils.h"
 
 #include "processors/processor_tracker_feature_trifocal.h"
-#include "processor_odom_3D.h"
-#include "capture_image.h"
-#include "sensor_camera.h"
+#include "base/processor/processor_odom_3D.h"
+#include "base/capture/capture_image.h"
+#include "base/sensor/sensor_camera.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -21,7 +21,6 @@ using namespace wolf;
 //        }
 //};
 
-
 //TEST(ProcessorTrackerFeatureTrifocal, Constructor)
 //{
 //  std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Constructor is empty." << std::endl;
@@ -146,7 +145,6 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
     }
 }
 
-
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 125d8a3a3..5d656eda2 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -9,8 +9,8 @@
 #include <Eigen/Geometry>
 
 //Wolf
-#include "wolf.h"
-#include "../rotations.h"
+#include "base/wolf.h"
+#include "base/rotations.h"
 
 //std
 #include <iostream>
@@ -27,7 +27,6 @@
 using namespace wolf;
 using namespace Eigen;
 
-
 TEST(exp_q, unit_norm)
 {
     Vector3s v0  = Vector3s::Random();
@@ -41,7 +40,6 @@ TEST(exp_q, unit_norm)
     }
 }
 
-
 TEST(rotations, pi2pi)
 {
     ASSERT_NEAR(M_PI_2, pi2pi((Scalar)M_PI_2), 1e-10);
@@ -219,7 +217,6 @@ TEST(rotations, AngleAxis_limits)
     }
 }
 
-
 TEST(compose, Quat_compos_const_rateOfTurn)
 {
     using namespace wolf;
@@ -480,7 +477,6 @@ TEST(Minus_and_diff, Random)
     q1              .coeffs().setRandom().normalize();
     q2              .coeffs().setRandom().normalize();
 
-
     Vector3s vr      = log_q(q1.conjugate() * q2);
     Vector3s vl      = log_q(q2 * q1.conjugate());
 
@@ -751,12 +747,10 @@ TEST(Conversions, e2R_R2q_q2e)
     ASSERT_MATRIX_APPROX(eo, e, 1e-10);
 }
 
-
 int main(int argc, char **argv)
 {
     using namespace wolf;
 
-
     /*
         LIST OF FUNCTIONS : 
         - pi2pi                                                            
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index aacedf976..cd809e75c 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -5,11 +5,10 @@
  *      \author: jsola
  */
 
-#include "sensor_base.h"
+#include "base/sensor/sensor_base.h"
 
 #include "utils_gtest.h"
 
-
 using namespace wolf;
 
 TEST(SensorBase, setNoiseStd)
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 99a9983db..cebdce1f8 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "node_base.h"
+#include "base/node_base.h"
 
 class CChildBase;
 
@@ -21,7 +21,6 @@ class CParentBase : public wolf::NodeBase
       }
 };
 
-
 class CParentDerived : public CParentBase
 {
    public:
@@ -51,8 +50,6 @@ class CChildDerived : public CChildBase
       CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){};
 };
 
-
-
 TEST(TestTest, SingleTest)
 {
     std::shared_ptr<CParentDerived> parent_derived_ptr = std::make_shared<CParentDerived>();
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 62bc03454..9d5fe610b 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -6,16 +6,16 @@
  */
 
 #include "utils_gtest.h"
-#include "../src/logging.h"
-
-#include "../problem.h"
-#include "../sensor_base.h"
-#include "../state_block.h"
-#include "../capture_void.h"
-#include "../constraint_pose_2D.h"
-#include "../solver/solver_manager.h"
-#include "../local_parametrization_base.h"
-#include "../local_parametrization_angle.h"
+#include "base/logging.h"
+
+#include "base/problem.h"
+#include "base/sensor/sensor_base.h"
+#include "base/state_block.h"
+#include "base/capture/capture_void.h"
+#include "base/constraint/constraint_pose_2D.h"
+#include "base/solver/solver_manager.h"
+#include "base/local_parametrization_base.h"
+#include "base/local_parametrization_angle.h"
 
 #include <iostream>
 
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index a5feaf39b..aad321781 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "../time_stamp.h"
+#include "base/time_stamp.h"
 
 #include <thread>
 
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 98eedac4d..9d65d8c0f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -7,7 +7,7 @@
 
 #include "utils_gtest.h"
 
-#include "track_matrix.h"
+#include "base/track_matrix.h"
 
 using namespace wolf;
 
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 92c848039..acbd4f242 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -5,13 +5,12 @@
  *      Author: jsola
  */
 
-
 #include "utils_gtest.h"
-#include "../src/logging.h"
+#include "base/logging.h"
 
-#include "../problem.h"
-#include "../trajectory_base.h"
-#include "../frame_base.h"
+#include "base/problem.h"
+#include "base/trajectory_base.h"
+#include "base/frame_base.h"
 
 #include <iostream>
 
@@ -56,7 +55,6 @@ struct DummyNotificationProcessor
   ProblemPtr problem_;
 };
 
-
 /// Set to true if you want debug info
 bool debug = false;
 
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
index 4c2bece6c..c114086a1 100644
--- a/test/processor_IMU_UnitTester.h
+++ b/test/processor_IMU_UnitTester.h
@@ -3,8 +3,8 @@
 #define PROCESSOR_IMU_UNITTESTER_H
 
 // Wolf
-#include "processor_IMU.h"
-#include "processor_motion.h"
+#include "base/processor/processor_IMU.h"
+#include "base/processor/processor_motion.h"
 
 namespace wolf {
     struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
@@ -65,7 +65,6 @@ namespace wolf {
             Eigen::Matrix3s dDv_dwb_;
             Eigen::Matrix3s dDq_dwb_;
 
-
         public:
             void copyfrom(IMU_jac_bias const& right){
 
@@ -197,9 +196,8 @@ namespace wolf {
 /////////////////////////////////////////////////////////
 
 // Wolf
-#include "state_block.h"
-#include "rotations.h"
-
+#include "base/state_block.h"
+#include "base/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 b47964a5d..8532cc9f2 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 "../../../wolf.h"
+#include "base/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 a124da37a..5ca60e7f1 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 "../../../wolf.h"
+#include "base/wolf.h"
 #include "../../utils_gtest.h"
 
 #include "../../../serialization/cereal/serialization_eigen_sparse.h"
diff --git a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
index 667c5d844..3df34a27a 100644
--- a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
+++ b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
@@ -198,7 +198,6 @@ TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrBIN)
   PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrBIN !\n");
 }
 
-
 //////////////////////////////////////
 /// LocalParametrizationQuaternion ///
 //////////////////////////////////////
diff --git a/test/utils_gtest.h b/test/utils_gtest.h
index 0883d86ff..9cb2166e8 100644
--- a/test/utils_gtest.h
+++ b/test/utils_gtest.h
@@ -11,7 +11,6 @@
 
 #include <gtest/gtest.h>
 
-
 // Macros for testing equalities and inequalities.
 //
 //    * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
@@ -48,7 +47,6 @@
 //
 //    * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
 
-
 // http://stackoverflow.com/a/29155677
 
 namespace testing
@@ -145,5 +143,4 @@ TEST(Test, Foo)
 
 } // namespace testing
 
-
 #endif /* WOLF_UTILS_GTEST_H */
diff --git a/wolf_scripts/include_refactor.sh b/wolf_scripts/include_refactor.sh
index bae2b8ef1..6d2c2fd8d 100755
--- a/wolf_scripts/include_refactor.sh
+++ b/wolf_scripts/include_refactor.sh
@@ -1,22 +1,21 @@
 #!/bin/bash
-# First parameter $1 is the path to the wolf root
-# Second parameter $2 is the name of the new folder
-
-hdrs_folder=$1/include/base/$2
-srcs_folder=$1/src/$2
-#Fix the includes in general
-core_headers=$(ag -g .*\\.h -l $1/include/base/$2/ | rev | cut -d '/' -f 1 | rev)
-for f in $core_headers; do
-    ch_files=$(ag \#include[[:space:]]+\"\(.*\/\)*"$2/$f"\" $1/include/base/ -l)
-    for fp in $ch_files; do
-        # echo ".h -> "$fp
-        sed -i -E "s/(#include[[:space:]]+\")("$2"\/)?"$f"\"/\1base\/"$2"\/"$f"\"/gp" $fp
-        sed -i -E "s/(#include[[:space:]]+<)("$2"\/)?"$f">/\1base\/"$2"\/"$f">/gp" $fp
+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
-    cs_files=$(ag \#include[[:space:]]+\"\(.*\/\)*"$2/$f"\" $1/src/ -l)
-    for fp in $cs_files; do
-        # echo ".cpp -> "$fp
-        sed -i -E "s/(#include[[:space:]]+\")("$2"\/)?"$f"\"/\1base\/"$2"\/"$f"\"/gp" $fp
-        sed -i -E "s/(#include[[:space:]]+<)("$2"\/)?"$f">/\1base\/"$2"\/"$f">/gp" $fp
-    done
-done
\ No newline at end of file
+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)
+#     if [ $matches -gt 1 ]; then
+#         echo $f " -> " $(echo $path | cut -d ' ' -f 1)
+#     fi
+# done
\ No newline at end of file
-- 
GitLab