diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7a44d2efaa6f776cbcc005355f63ce925176a0c7..ed049c6c3ca28d23051e96a5ee44336f584c8df7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -106,157 +106,9 @@ ENDIF()
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
 
-# ============ HEADERS ============ 
-SET(HDRS_CAPTURE
-  include/${PROJECT_NAME}/capture/capture_base.h
-  include/${PROJECT_NAME}/capture/capture_diff_drive.h
-  include/${PROJECT_NAME}/capture/capture_landmarks_external.h
-  include/${PROJECT_NAME}/capture/capture_motion.h
-  include/${PROJECT_NAME}/capture/capture_odom_2d.h
-  include/${PROJECT_NAME}/capture/capture_odom_3d.h
-  include/${PROJECT_NAME}/capture/capture_pose.h
-  include/${PROJECT_NAME}/capture/capture_void.h
-  )
-SET(HDRS_COMMON
-  include/${PROJECT_NAME}/common/factory.h
-  include/${PROJECT_NAME}/common/node_base.h
-  include/${PROJECT_NAME}/common/node_state_blocks.h
-  include/${PROJECT_NAME}/common/time_stamp.h
-  include/${PROJECT_NAME}/common/wolf.h
-  )
-SET(HDRS_COMPOSITE
-  include/${PROJECT_NAME}/composite/composite.h
-  include/${PROJECT_NAME}/composite/prior_composite.h
-  include/${PROJECT_NAME}/composite/type_composite.h
-  include/${PROJECT_NAME}/composite/vector_composite.h
-  )
-SET(HDRS_FACTOR
-  include/${PROJECT_NAME}/factor/factor_analytic.h
-  include/${PROJECT_NAME}/factor/factor_autodiff.h
-  include/${PROJECT_NAME}/factor/factor_base.h
-  include/${PROJECT_NAME}/factor/factor_block_absolute.h
-  include/${PROJECT_NAME}/factor/factor_block_difference.h
-  include/${PROJECT_NAME}/factor/factor_diff_drive.h
-  include/${PROJECT_NAME}/factor/factor_distance_3d.h
-  include/${PROJECT_NAME}/factor/factor_pose_2d.h
-  include/${PROJECT_NAME}/factor/factor_pose_2d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_pose_3d.h
-  include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
-  include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
-  include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_2d.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_3d.h
-  include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h
-  include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
-  )
-SET(HDRS_FEATURE
-  include/${PROJECT_NAME}/feature/feature_base.h
-  include/${PROJECT_NAME}/feature/feature_diff_drive.h
-  include/${PROJECT_NAME}/feature/feature_landmark_external.h
-  include/${PROJECT_NAME}/feature/feature_match.h
-  include/${PROJECT_NAME}/feature/feature_motion.h
-  include/${PROJECT_NAME}/feature/feature_odom_2d.h
-  include/${PROJECT_NAME}/feature/feature_pose.h
-  )
-SET(HDRS_FRAME
-  include/${PROJECT_NAME}/frame/frame_base.h
-  )
-SET(HDRS_HARDWARE
-  include/${PROJECT_NAME}/hardware/hardware_base.h
-  )
-SET(HDRS_LANDMARK
-  include/${PROJECT_NAME}/landmark/factory_landmark.h
-  include/${PROJECT_NAME}/landmark/landmark_base.h
-  include/${PROJECT_NAME}/landmark/landmark_match.h
-  include/${PROJECT_NAME}/landmark/landmark.h
-  )
-SET(HDRS_MATH
-  include/${PROJECT_NAME}/math/SE2.h
-  include/${PROJECT_NAME}/math/SE3.h
-  include/${PROJECT_NAME}/math/rotations.h
-  include/${PROJECT_NAME}/math/covariance.h
-  )
-SET(HDRS_MAP
-  include/${PROJECT_NAME}/map/factory_map.h
-  include/${PROJECT_NAME}/map/map_base.h
-  )
-SET(HDRS_PROBLEM
-  include/${PROJECT_NAME}/problem/problem.h
-  )
-SET(HDRS_PROCESSOR
-  include/${PROJECT_NAME}/processor/buffer.h
-  include/${PROJECT_NAME}/processor/factory_processor.h
-  include/${PROJECT_NAME}/processor/motion_buffer.h
-  include/${PROJECT_NAME}/processor/motion_provider.h
-  include/${PROJECT_NAME}/processor/processor_base.h
-  include/${PROJECT_NAME}/processor/processor_diff_drive.h
-  include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
-  include/${PROJECT_NAME}/processor/processor_landmark_external.h
-  include/${PROJECT_NAME}/processor/processor_loop_closure.h
-  include/${PROJECT_NAME}/processor/processor_motion.h
-  include/${PROJECT_NAME}/processor/processor_odom_2d.h
-  include/${PROJECT_NAME}/processor/processor_odom_3d.h
-  include/${PROJECT_NAME}/processor/processor_pose.h
-  include/${PROJECT_NAME}/processor/processor_tracker.h
-  include/${PROJECT_NAME}/processor/processor_tracker_feature.h
-  include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
-  include/${PROJECT_NAME}/processor/track_matrix.h
-  )
-SET(HDRS_SENSOR
-  include/${PROJECT_NAME}/sensor/factory_sensor.h
-  include/${PROJECT_NAME}/sensor/sensor_base.h
-  include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
-  include/${PROJECT_NAME}/sensor/sensor_motion_model.h
-  include/${PROJECT_NAME}/sensor/sensor_odom.h
-  include/${PROJECT_NAME}/sensor/sensor_pose.h
-  )
-SET(HDRS_SOLVER
-  include/${PROJECT_NAME}/solver/factory_solver.h
-  include/${PROJECT_NAME}/solver/solver_manager.h
-  )
-SET(HDRS_STATE_BLOCK
-  include/${PROJECT_NAME}/state_block/factory_state_block.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_base.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
-  include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
-  include/${PROJECT_NAME}/state_block/state_angle.h
-  include/${PROJECT_NAME}/state_block/state_block.h
-  include/${PROJECT_NAME}/state_block/state_block_derived.h
-  include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
-  include/${PROJECT_NAME}/state_block/state_quaternion.h
-  )
-SET(HDRS_TRAJECTORY
-  include/${PROJECT_NAME}/trajectory/trajectory_base.h
-  )
-SET(HDRS_TREE_MANAGER
-  include/${PROJECT_NAME}/tree_manager/factory_tree_manager.h
-  include/${PROJECT_NAME}/tree_manager/tree_manager_base.h
-  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window.h
-  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window_dual_rate.h
-  )
-SET(HDRS_UTILS
-  include/${PROJECT_NAME}/utils/check_log.h
-  include/${PROJECT_NAME}/utils/eigen_assert.h
-  include/${PROJECT_NAME}/utils/folder_registry.h
-  include/${PROJECT_NAME}/utils/graph_search.h
-  include/${PROJECT_NAME}/utils/loader.h
-  include/${PROJECT_NAME}/utils/loader_utils.h
-  include/${PROJECT_NAME}/utils/load_core.h
-  include/${PROJECT_NAME}/utils/logging.h
-  include/${PROJECT_NAME}/utils/singleton.h
-  include/${PROJECT_NAME}/utils/string_utils.h
-  include/${PROJECT_NAME}/utils/unused.h
-  include/${PROJECT_NAME}/utils/utils_gtest.h
-  )
-  
 # ============ SOURCES ============ 
-SET(SRCS_CAPTURE
+SET(SRCS
+  # capture
   src/capture/capture_base.cpp
   src/capture/capture_diff_drive.cpp
   src/capture/capture_landmarks_external.cpp
@@ -265,45 +117,35 @@ SET(SRCS_CAPTURE
   src/capture/capture_odom_3d.cpp
   src/capture/capture_pose.cpp
   src/capture/capture_void.cpp
-  )
-SET(SRCS_COMMON
+  # common
   src/common/node_base.cpp
   src/common/node_state_blocks.cpp
   src/common/time_stamp.cpp
-  )
-SET(SRCS_COMPOSITE
+  # composite
   src/composite/prior_composite.cpp
   src/composite/vector_composite.cpp
-  )
-SET(SRCS_FACTOR
+  # factor
   src/factor/factor_analytic.cpp
   src/factor/factor_base.cpp
-  )
-SET(SRCS_FEATURE
+  # feature
   src/feature/feature_base.cpp
   src/feature/feature_diff_drive.cpp
   src/feature/feature_landmark_external.cpp
   src/feature/feature_motion.cpp
   src/feature/feature_odom_2d.cpp
   src/feature/feature_pose.cpp
-  )
-SET(SRCS_FRAME
+  # frame
   src/frame/frame_base.cpp
-  )
-SET(SRCS_HARDWARE
+  # hardware
   src/hardware/hardware_base.cpp
-  )
-SET(SRCS_LANDMARK
+  # landmark
   src/landmark/landmark_base.cpp
   src/landmark/landmark.cpp
-  )
-SET(SRCS_MAP
+  # map
   src/map/map_base.cpp
-  )
-SET(SRCS_PROBLEM
+  # problem
   src/problem/problem.cpp
-  )
-SET(SRCS_PROCESSOR
+  # processor
   src/processor/motion_buffer.cpp
   src/processor/motion_provider.cpp
   src/processor/processor_base.cpp
@@ -319,32 +161,26 @@ SET(SRCS_PROCESSOR
   src/processor/processor_tracker_feature.cpp
   src/processor/processor_tracker_landmark.cpp
   src/processor/track_matrix.cpp
-  )
-SET(SRCS_SENSOR
+  # sensor
   src/sensor/sensor_base.cpp
   src/sensor/sensor_diff_drive.cpp
   src/sensor/sensor_motion_model.cpp
   src/sensor/sensor_odom.cpp
   src/sensor/sensor_pose.cpp
-  )
-SET(SRCS_SOLVER
+  # solver
   src/solver/solver_manager.cpp
-  )
-SET(SRCS_STATE_BLOCK
+  # state_block
   src/state_block/local_parametrization_base.cpp
   src/state_block/local_parametrization_homogeneous.cpp
   src/state_block/local_parametrization_quaternion.cpp
   src/state_block/state_block.cpp
   src/state_block/state_block_derived.cpp
-  )
-SET(SRCS_TRAJECTORY
+  # trajectory
   src/trajectory/trajectory_base.cpp
-  )
-SET(SRCS_TREE_MANAGER
+  # tree_manager
   src/tree_manager/tree_manager_sliding_window.cpp
   src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
-  )
-SET(SRCS_UTILS
+  # utils
   src/utils/check_log.cpp
   src/utils/graph_search.cpp
   src/utils/loader.cpp
@@ -354,47 +190,15 @@ SET(SRCS_UTILS
   
 # ============ OPTIONALS ============ 
 IF (Ceres_FOUND)
-    SET(HDRS_SOLVER_CERES
-      include/${PROJECT_NAME}/ceres_wrapper/cost_function_wrapper.h
-      include/${PROJECT_NAME}/ceres_wrapper/create_numeric_diff_cost_function.h
-      include/${PROJECT_NAME}/ceres_wrapper/local_parametrization_wrapper.h
-      include/${PROJECT_NAME}/ceres_wrapper/iteration_update_callback.h
-      include/${PROJECT_NAME}/ceres_wrapper/solver_ceres.h
-      include/${PROJECT_NAME}/ceres_wrapper/wolf_jet.h
-      include/${PROJECT_NAME}/solver/solver_manager.h
-      )
-    SET(SRCS_SOLVER_CERES
-      src/ceres_wrapper/solver_ceres.cpp
-      src/ceres_wrapper/local_parametrization_wrapper.cpp
-      src/solver/solver_manager.cpp
-      )
-ELSE(Ceres_FOUND)
-	SET(HDRS_SOLVER_CERES)
-	SET(SRCS_SOLVER_CERES)
+  SET(SRCS ${SRCS}
+    src/ceres_wrapper/solver_ceres.cpp
+    src/ceres_wrapper/local_parametrization_wrapper.cpp
+    src/solver/solver_manager.cpp
+    )
 ENDIF(Ceres_FOUND)
 
 # create the shared library
-ADD_LIBRARY(${PLUGIN_NAME}
-  SHARED
-  ${SRCS_CAPTURE}
-  ${SRCS_COMMON}
-  ${SRCS_COMPOSITE}
-  ${SRCS_FACTOR}
-  ${SRCS_FEATURE}
-  ${SRCS_FRAME}
-  ${SRCS_HARDWARE}
-  ${SRCS_LANDMARK}
-  ${SRCS_MAP}
-  ${SRCS_PROBLEM}
-  ${SRCS_PROCESSOR}
-  ${SRCS_SENSOR}
-  ${SRCS_SOLVER}
-  ${SRCS_SOLVER_CERES}
-  ${SRCS_STATE_BLOCK}
-  ${SRCS_TRAJECTORY}
-  ${SRCS_TREE_MANAGER}
-  ${SRCS_UTILS}
-  )
+ADD_LIBRARY(${PLUGIN_NAME} SHARED ${SRCS})
 
 # Set compiler options
 # ====================  
@@ -466,44 +270,8 @@ target_include_directories(${PLUGIN_NAME} PUBLIC
 )
 
 #install headers
-INSTALL(FILES ${HDRS_CAPTURE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
-INSTALL(FILES ${HDRS_COMMON}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/common)
-INSTALL(FILES ${HDRS_COMPOSITE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/composite)
-INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
-INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_FRAME}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/frame)
-INSTALL(FILES ${HDRS_HARDWARE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/hardware)
-INSTALL(FILES ${HDRS_LANDMARK}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/landmark)
-INSTALL(FILES ${HDRS_MAP}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/map)
-INSTALL(FILES ${HDRS_MATH}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
-INSTALL(FILES ${HDRS_PROBLEM}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/problem)
-INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
-INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
-INSTALL(FILES ${HDRS_SOLVER}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver)
-INSTALL(FILES ${HDRS_SOLVER_CERES}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/ceres_wrapper)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_TRAJECTORY}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/trajectory)
-INSTALL(FILES ${HDRS_TREE_MANAGER}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
-INSTALL(FILES ${HDRS_UTILS}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
+INSTALL(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION ${INCLUDE_INSTALL_DIR})
 
 #install config.h
 INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h