diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6ffe566eb407fd0ee5ade59a5f0c0ceca5f711d9..b44f68d79c1ea47dd5b9cc8879b6b98ab44e7f6a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -254,413 +254,412 @@ ENDIF(GLOG_FOUND)
 #HEADERS
 
 SET(HDRS_BASE
-include/base/diff_drive_tools.h
-include/base/diff_drive_tools.hpp
-include/base/eigen_assert.h
-include/base/eigen_predicates.h
-include/base/factory.h
-include/base/frame_base.h
-include/base/hardware_base.h
-include/base/IMU_tools.h
-include/base/local_parametrization_angle.h
-include/base/local_parametrization_base.h
-include/base/local_parametrization_homogeneous.h
-include/base/local_parametrization_polyline_extreme.h
-include/base/local_parametrization_quaternion.h
-include/base/logging.h
-include/base/make_unique.h
-include/base/map_base.h
-include/base/motion_buffer.h
-include/base/node_base.h
-include/base/pinhole_tools.h
-include/base/problem.h
-include/base/rotations.h
-include/base/singleton.h
-include/base/state_angle.h
-include/base/state_block.h
-include/base/state_homogeneous_3D.h
-include/base/state_quaternion.h
-include/base/SE3.h
-include/base/time_stamp.h
-include/base/track_matrix.h
-include/base/trajectory_base.h
-include/base/wolf.h
-include/base/IMU_tools.h
-include/base/local_parametrization_polyline_extreme.h
+  include/base/diff_drive_tools.h
+  include/base/diff_drive_tools.hpp
+  include/base/eigen_assert.h
+  include/base/eigen_predicates.h
+  include/base/factory.h
+  include/base/frame_base.h
+  include/base/hardware_base.h
+  include/base/IMU_tools.h
+  include/base/local_parametrization_angle.h
+  include/base/local_parametrization_base.h
+  include/base/local_parametrization_homogeneous.h
+  include/base/local_parametrization_polyline_extreme.h
+  include/base/local_parametrization_quaternion.h
+  include/base/logging.h
+  include/base/make_unique.h
+  include/base/map_base.h
+  include/base/motion_buffer.h
+  include/base/node_base.h
+  include/base/pinhole_tools.h
+  include/base/problem.h
+  include/base/rotations.h
+  include/base/singleton.h
+  include/base/state_angle.h
+  include/base/state_block.h
+  include/base/state_homogeneous_3D.h
+  include/base/state_quaternion.h
+  include/base/SE3.h
+  include/base/time_stamp.h
+  include/base/track_matrix.h
+  include/base/trajectory_base.h
+  include/base/wolf.h
+  include/base/IMU_tools.h
+  include/base/local_parametrization_polyline_extreme.h
   )
 SET(HDRS_CAPTURE
-include/base/capture/capture_base.h
-include/base/capture/capture_buffer.h
-include/base/capture/capture_pose.h
-include/base/capture/capture_void.h
-include/base/capture/capture_motion.h
-include/base/capture/capture_GPS_fix.h
-include/base/capture/capture_IMU.h
-include/base/capture/capture_odom_2D.h
-include/base/capture/capture_odom_3D.h
-include/base/capture/capture_GPS_fix.h
-include/base/capture/capture_IMU.h
-include/base/capture/capture_odom_2D.h
-include/base/capture/capture_odom_3D.h
-include/base/capture/capture_velocity.h
-include/base/capture/capture_wheel_joint_position.h
-    )
-  SET(HDRS_CONSTRAINT
-include/base/constraint/constraint_block_absolute.h
-include/base/constraint/constraint_container.h
-include/base/constraint/constraint_corner_2D.h
-include/base/constraint/constraint_AHP.h
-include/base/constraint/constraint_epipolar.h
-include/base/constraint/constraint_IMU.h
-include/base/constraint/constraint_fix_bias.h
-include/base/constraint/constraint_GPS_2D.h
-include/base/constraint/constraint_GPS_pseudorange_3D.h
-include/base/constraint/constraint_GPS_pseudorange_2D.h
-include/base/constraint/constraint_odom_2D.h
-include/base/constraint/constraint_odom_2D_analytic.h
-include/base/constraint/constraint_odom_3D.h
-include/base/constraint/constraint_point_2D.h
-include/base/constraint/constraint_point_to_line_2D.h
-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
-include/base/constraint/constraint_autodiff_trifocal.h
-include/base/constraint/constraint_autodiff_distance_3D.h
-include/base/constraint/constraint_AHP.h
-include/base/constraint/constraint_block_absolute.h
-include/base/constraint/constraint_container.h
-include/base/constraint/constraint_corner_2D.h
-include/base/constraint/constraint_diff_drive.h
-include/base/constraint/constraint_epipolar.h
-include/base/constraint/constraint_IMU.h
-include/base/constraint/constraint_fix_bias.h
-include/base/constraint/constraint_GPS_2D.h
-include/base/constraint/constraint_GPS_pseudorange_3D.h
-include/base/constraint/constraint_GPS_pseudorange_2D.h
-include/base/constraint/constraint_odom_2D.h
-include/base/constraint/constraint_odom_2D_analytic.h
-include/base/constraint/constraint_odom_3D.h
-include/base/constraint/constraint_point_2D.h
-include/base/constraint/constraint_point_to_line_2D.h
-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
-include/base/constraint/constraint_analytic.h
-include/base/constraint/constraint_autodiff.h
-include/base/constraint/constraint_base.h
-    )
-  SET(HDRS_FEATURE
-include/base/feature/feature_corner_2D.h
-include/base/feature/feature_GPS_fix.h
-include/base/feature/feature_GPS_pseudorange.h
-include/base/feature/feature_IMU.h
-include/base/feature/feature_odom_2D.h
-include/base/feature/feature_polyline_2D.h
-include/base/feature/feature_corner_2D.h
-include/base/feature/feature_diff_drive.h
-include/base/feature/feature_GPS_fix.h
-include/base/feature/feature_GPS_pseudorange.h
-include/base/feature/feature_IMU.h
-include/base/feature/feature_odom_2D.h
-include/base/feature/feature_polyline_2D.h
-include/base/feature/feature_base.h
-include/base/feature/feature_match.h
-include/base/feature/feature_pose.h
-    )
-  SET(HDRS_LANDMARK
-include/base/landmark/landmark_base.h
-include/base/landmark/landmark_match.h
-include/base/landmark/landmark_corner_2D.h
-include/base/landmark/landmark_container.h
-include/base/landmark/landmark_line_2D.h
-include/base/landmark/landmark_polyline_2D.h
-include/base/landmark/landmark_corner_2D.h
-include/base/landmark/landmark_container.h
-include/base/landmark/landmark_line_2D.h
-include/base/landmark/landmark_polyline_2D.h
-    )
-  SET(HDRS_PROCESSOR
-include/base/processor/processor_capture_holder.h
-include/base/processor/processor_diff_drive.h
-include/base/processor/processor_frame_nearest_neighbor_filter.h
-include/base/processor/processor_IMU.h
-include/base/processor/processor_odom_2D.h
-include/base/processor/processor_odom_3D.h
-include/base/processor/processor_tracker_feature_dummy.h
-include/base/processor/processor_tracker_landmark.h
-include/base/processor/processor_tracker_landmark_dummy.h
-include/base/processor/processor_frame_nearest_neighbor_filter.h
-include/base/processor/processor_IMU.h
-include/base/processor/processor_odom_2D.h
-include/base/processor/processor_odom_3D.h
-include/base/processor/processor_tracker_feature_dummy.h
-include/base/processor/processor_tracker_landmark_dummy.h
-include/base/processor/processor_factory.h
-include/base/processor/processor_logging.h
-include/base/processor/processor_base.h
-include/base/processor/processor_factory.h
-include/base/processor/processor_loopclosure_base.h
-include/base/processor/processor_motion.h
-include/base/processor/processor_tracker_feature.h
-include/base/processor/processor_tracker.h
-    )
-  SET(HDRS_SENSOR
-include/base/sensor/sensor_base.h
-include/base/sensor/sensor_camera.h
-include/base/sensor/sensor_diff_drive.h
-include/base/sensor/sensor_GPS.h
-include/base/sensor/sensor_GPS_fix.h
-include/base/sensor/sensor_IMU.h
-include/base/sensor/sensor_odom_2D.h
-include/base/sensor/sensor_odom_3D.h
-include/base/sensor/sensor_camera.h
-include/base/sensor/sensor_GPS.h
-include/base/sensor/sensor_GPS_fix.h
-include/base/sensor/sensor_IMU.h
-include/base/sensor/sensor_odom_2D.h
-include/base/sensor/sensor_odom_3D.h
-include/base/sensor/sensor_factory.h
-    )
+  include/base/capture/capture_base.h
+  include/base/capture/capture_buffer.h
+  include/base/capture/capture_pose.h
+  include/base/capture/capture_void.h
+  include/base/capture/capture_motion.h
+  include/base/capture/capture_GPS_fix.h
+  include/base/capture/capture_IMU.h
+  include/base/capture/capture_odom_2D.h
+  include/base/capture/capture_odom_3D.h
+  include/base/capture/capture_GPS_fix.h
+  include/base/capture/capture_IMU.h
+  include/base/capture/capture_odom_2D.h
+  include/base/capture/capture_odom_3D.h
+  include/base/capture/capture_velocity.h
+  include/base/capture/capture_wheel_joint_position.h
+  )
+SET(HDRS_CONSTRAINT
+  include/base/constraint/constraint_block_absolute.h
+  include/base/constraint/constraint_container.h
+  include/base/constraint/constraint_corner_2D.h
+  include/base/constraint/constraint_AHP.h
+  include/base/constraint/constraint_epipolar.h
+  include/base/constraint/constraint_IMU.h
+  include/base/constraint/constraint_fix_bias.h
+  include/base/constraint/constraint_GPS_2D.h
+  include/base/constraint/constraint_GPS_pseudorange_3D.h
+  include/base/constraint/constraint_GPS_pseudorange_2D.h
+  include/base/constraint/constraint_odom_2D.h
+  include/base/constraint/constraint_odom_2D_analytic.h
+  include/base/constraint/constraint_odom_3D.h
+  include/base/constraint/constraint_point_2D.h
+  include/base/constraint/constraint_point_to_line_2D.h
+  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
+  include/base/constraint/constraint_autodiff_trifocal.h
+  include/base/constraint/constraint_autodiff_distance_3D.h
+  include/base/constraint/constraint_AHP.h
+  include/base/constraint/constraint_block_absolute.h
+  include/base/constraint/constraint_container.h
+  include/base/constraint/constraint_corner_2D.h
+  include/base/constraint/constraint_diff_drive.h
+  include/base/constraint/constraint_epipolar.h
+  include/base/constraint/constraint_IMU.h
+  include/base/constraint/constraint_fix_bias.h
+  include/base/constraint/constraint_GPS_2D.h
+  include/base/constraint/constraint_GPS_pseudorange_3D.h
+  include/base/constraint/constraint_GPS_pseudorange_2D.h
+  include/base/constraint/constraint_odom_2D.h
+  include/base/constraint/constraint_odom_2D_analytic.h
+  include/base/constraint/constraint_odom_3D.h
+  include/base/constraint/constraint_point_2D.h
+  include/base/constraint/constraint_point_to_line_2D.h
+  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
+  include/base/constraint/constraint_analytic.h
+  include/base/constraint/constraint_autodiff.h
+  include/base/constraint/constraint_base.h
+  )
+SET(HDRS_FEATURE
+  include/base/feature/feature_corner_2D.h
+  include/base/feature/feature_GPS_fix.h
+  include/base/feature/feature_GPS_pseudorange.h
+  include/base/feature/feature_IMU.h
+  include/base/feature/feature_odom_2D.h
+  include/base/feature/feature_polyline_2D.h
+  include/base/feature/feature_corner_2D.h
+  include/base/feature/feature_diff_drive.h
+  include/base/feature/feature_GPS_fix.h
+  include/base/feature/feature_GPS_pseudorange.h
+  include/base/feature/feature_IMU.h
+  include/base/feature/feature_odom_2D.h
+  include/base/feature/feature_polyline_2D.h
+  include/base/feature/feature_base.h
+  include/base/feature/feature_match.h
+  include/base/feature/feature_pose.h
+  )
+SET(HDRS_LANDMARK
+  include/base/landmark/landmark_base.h
+  include/base/landmark/landmark_match.h
+  include/base/landmark/landmark_corner_2D.h
+  include/base/landmark/landmark_container.h
+  include/base/landmark/landmark_line_2D.h
+  include/base/landmark/landmark_polyline_2D.h
+  include/base/landmark/landmark_corner_2D.h
+  include/base/landmark/landmark_container.h
+  include/base/landmark/landmark_line_2D.h
+  include/base/landmark/landmark_polyline_2D.h
+  )
+SET(HDRS_PROCESSOR
+  include/base/processor/processor_capture_holder.h
+  include/base/processor/processor_diff_drive.h
+  include/base/processor/processor_frame_nearest_neighbor_filter.h
+  include/base/processor/processor_IMU.h
+  include/base/processor/processor_odom_2D.h
+  include/base/processor/processor_odom_3D.h
+  include/base/processor/processor_tracker_feature_dummy.h
+  include/base/processor/processor_tracker_landmark.h
+  include/base/processor/processor_tracker_landmark_dummy.h
+  include/base/processor/processor_frame_nearest_neighbor_filter.h
+  include/base/processor/processor_IMU.h
+  include/base/processor/processor_odom_2D.h
+  include/base/processor/processor_odom_3D.h
+  include/base/processor/processor_tracker_feature_dummy.h
+  include/base/processor/processor_tracker_landmark_dummy.h
+  include/base/processor/processor_factory.h
+  include/base/processor/processor_logging.h
+  include/base/processor/processor_base.h
+  include/base/processor/processor_factory.h
+  include/base/processor/processor_loopclosure_base.h
+  include/base/processor/processor_motion.h
+  include/base/processor/processor_tracker_feature.h
+  include/base/processor/processor_tracker.h
+  )
+SET(HDRS_SENSOR
+  include/base/sensor/sensor_base.h
+  include/base/sensor/sensor_camera.h
+  include/base/sensor/sensor_diff_drive.h
+  include/base/sensor/sensor_GPS.h
+  include/base/sensor/sensor_GPS_fix.h
+  include/base/sensor/sensor_IMU.h
+  include/base/sensor/sensor_odom_2D.h
+  include/base/sensor/sensor_odom_3D.h
+  include/base/sensor/sensor_camera.h
+  include/base/sensor/sensor_GPS.h
+  include/base/sensor/sensor_GPS_fix.h
+  include/base/sensor/sensor_IMU.h
+  include/base/sensor/sensor_odom_2D.h
+  include/base/sensor/sensor_odom_3D.h
+  include/base/sensor/sensor_factory.h
+  )
 SET(HDRS_SOLVER
-include/base/solver/solver_manager.h
-)
+  include/base/solver/solver_manager.h
+  )
 # [Add generic derived header before this line]
 
 SET(HDRS_DTASSC
-include/base/track_matrix.h
-include/base/association/association_solver.h
-include/base/association/association_node.h
-include/base/association/association_tree.h
-include/base/association/association_nnls.h
-    )
+  include/base/track_matrix.h
+  include/base/association/association_solver.h
+  include/base/association/association_node.h
+  include/base/association/association_tree.h
+  include/base/association/association_nnls.h
+  )
 
 SET(HDRS_CORE 
-include/base/capture/capture_base.h
-include/base/capture/capture_buffer.h
-include/base/capture/capture_pose.h
-include/base/capture/capture_void.h
-include/base/constraint/constraint_analytic.h
-include/base/constraint/constraint_autodiff.h
-include/base/constraint/constraint_base.h
-include/base/processor/processor_factory.h
-include/base/feature/feature_base.h
-include/base/feature/feature_match.h
-include/base/feature/feature_pose.h
-include/base/frame_base.h
-include/base/hardware_base.h
-include/base/landmark/landmark_base.h
-include/base/local_parametrization_angle.h
-include/base/local_parametrization_base.h
-include/base/local_parametrization_homogeneous.h
-include/base/local_parametrization_quaternion.h
-include/base/processor/processor_logging.h
-include/base/map_base.h
-include/base/motion_buffer.h
-include/base/node_base.h
-include/base/problem.h
-include/base/processor/processor_base.h
-include/base/processor/processor_loopclosure_base.h
-include/base/processor/processor_motion.h
-include/base/processor/processor_tracker_feature.h
-include/base/processor/processor_tracker.h
-include/base/rotations.h
-include/base/sensor/sensor_base.h
-include/base/processor/processor_factory.h
-include/base/singleton.h
-include/base/state_angle.h
-include/base/state_block.h
-include/base/state_homogeneous_3D.h
-include/base/state_quaternion.h
-include/base/SE3.h
-include/base/time_stamp.h
-include/base/track_matrix.h
-include/base/trajectory_base.h
-include/base/wolf.h
+  include/base/capture/capture_base.h
+  include/base/capture/capture_buffer.h
+  include/base/capture/capture_pose.h
+  include/base/capture/capture_void.h
+  include/base/constraint/constraint_analytic.h
+  include/base/constraint/constraint_autodiff.h
+  include/base/constraint/constraint_base.h
+  include/base/processor/processor_factory.h
+  include/base/feature/feature_base.h
+  include/base/feature/feature_match.h
+  include/base/feature/feature_pose.h
+  include/base/frame_base.h
+  include/base/hardware_base.h
+  include/base/landmark/landmark_base.h
+  include/base/local_parametrization_angle.h
+  include/base/local_parametrization_base.h
+  include/base/local_parametrization_homogeneous.h
+  include/base/local_parametrization_quaternion.h
+  include/base/processor/processor_logging.h
+  include/base/map_base.h
+  include/base/motion_buffer.h
+  include/base/node_base.h
+  include/base/problem.h
+  include/base/processor/processor_base.h
+  include/base/processor/processor_loopclosure_base.h
+  include/base/processor/processor_motion.h
+  include/base/processor/processor_tracker_feature.h
+  include/base/processor/processor_tracker.h
+  include/base/rotations.h
+  include/base/sensor/sensor_base.h
+  include/base/processor/processor_factory.h
+  include/base/singleton.h
+  include/base/state_angle.h
+  include/base/state_block.h
+  include/base/state_homogeneous_3D.h
+  include/base/state_quaternion.h
+  include/base/SE3.h
+  include/base/time_stamp.h
+  include/base/track_matrix.h
+  include/base/trajectory_base.h
+  include/base/wolf.h
   )
 
 #SOURCES
 SET(SRCS_CORE 
-src/capture/capture_base.cpp
-src/capture/capture_pose.cpp
-src/capture/capture_void.cpp
-src/constraint/constraint_analytic.cpp
-src/constraint/constraint_base.cpp
-src/feature/feature_base.cpp
-src/feature/feature_pose.cpp
-src/frame_base.cpp
-src/hardware_base.cpp
-src/landmark/landmark_base.cpp
-src/local_parametrization_base.cpp
-src/local_parametrization_homogeneous.cpp
-src/local_parametrization_quaternion.cpp
-src/map_base.cpp
-src/motion_buffer.cpp
-src/node_base.cpp
-src/problem.cpp
-src/processor/processor_base.cpp
-src/processor/processor_loopclosure_base.cpp
-src/processor/processor_motion.cpp
-src/processor/processor_tracker.cpp
-src/sensor/sensor_base.cpp
-src/state_block.cpp
-src/time_stamp.cpp
-src/track_matrix.cpp
-src/trajectory_base.cpp
+  src/capture/capture_base.cpp
+  src/capture/capture_pose.cpp
+  src/capture/capture_void.cpp
+  src/constraint/constraint_analytic.cpp
+  src/constraint/constraint_base.cpp
+  src/feature/feature_base.cpp
+  src/feature/feature_pose.cpp
+  src/frame_base.cpp
+  src/hardware_base.cpp
+  src/landmark/landmark_base.cpp
+  src/local_parametrization_base.cpp
+  src/local_parametrization_homogeneous.cpp
+  src/local_parametrization_quaternion.cpp
+  src/map_base.cpp
+  src/motion_buffer.cpp
+  src/node_base.cpp
+  src/problem.cpp
+  src/processor/processor_base.cpp
+  src/processor/processor_loopclosure_base.cpp
+  src/processor/processor_motion.cpp
+  src/processor/processor_tracker.cpp
+  src/sensor/sensor_base.cpp
+  src/state_block.cpp
+  src/time_stamp.cpp
+  src/track_matrix.cpp
+  src/trajectory_base.cpp
   )
 
 SET(SRCS_BASE
-src/capture/capture_motion.cpp
-src/processor/processor_capture_holder.cpp
-# examples/test_processor_tracker_landmark.cpp
-    )
+  src/capture/capture_motion.cpp
+  src/processor/processor_capture_holder.cpp
+  # examples/test_processor_tracker_landmark.cpp
+  )
 
 SET(SRCS
-src/local_parametrization_polyline_extreme.cpp
-test/processor_IMU_UnitTester.cpp
+  src/local_parametrization_polyline_extreme.cpp
+  test/processor_IMU_UnitTester.cpp
+  )
+SET(SRCS_CAPTURE
+  src/capture/capture_GPS_fix.cpp
+  src/capture/capture_IMU.cpp
+  src/capture/capture_odom_2D.cpp
+  src/capture/capture_odom_3D.cpp
+  src/capture/capture_velocity.cpp
+  src/capture/capture_wheel_joint_position.cpp
+  )
+SET(SRCS_FEATURE
+  src/feature/feature_corner_2D.cpp
+  src/feature/feature_diff_drive.cpp
+  src/feature/feature_GPS_fix.cpp
+  src/feature/feature_GPS_pseudorange.cpp
+  src/feature/feature_IMU.cpp
+  src/feature/feature_odom_2D.cpp
+  src/feature/feature_polyline_2D.cpp
+  )
+SET(SRCS_LANDMARK
+  src/landmark/landmark_corner_2D.cpp
+  src/landmark/landmark_container.cpp
+  src/landmark/landmark_line_2D.cpp
+  src/landmark/landmark_polyline_2D.cpp
+  )
+SET(SRCS_PROCESSOR
+  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
+  src/processor/processor_odom_3D.cpp
+  src/processor/processor_tracker_feature.cpp
+  src/processor/processor_tracker_feature_dummy.cpp
+  src/processor/processor_tracker_landmark_dummy.cpp
+  src/processor/processor_tracker_landmark.cpp
+  )
+SET(SRCS_SENSOR
+  src/sensor/sensor_camera.cpp
+  src/sensor/sensor_diff_drive.cpp
+  src/sensor/sensor_GPS.cpp
+  src/sensor/sensor_GPS_fix.cpp
+  src/sensor/sensor_IMU.cpp
+  src/sensor/sensor_odom_2D.cpp
+  src/sensor/sensor_odom_3D.cpp
   )
-  SET(SRCS_CAPTURE
-src/capture/capture_GPS_fix.cpp
-src/capture/capture_IMU.cpp
-src/capture/capture_odom_2D.cpp
-src/capture/capture_odom_3D.cpp
-src/capture/capture_velocity.cpp
-src/capture/capture_wheel_joint_position.cpp
-    )
-  SET(SRCS_FEATURE
-src/feature/feature_corner_2D.cpp
-src/feature/feature_diff_drive.cpp
-src/feature/feature_GPS_fix.cpp
-src/feature/feature_GPS_pseudorange.cpp
-src/feature/feature_IMU.cpp
-src/feature/feature_odom_2D.cpp
-src/feature/feature_polyline_2D.cpp
-    )
-  SET(SRCS_LANDMARK
-src/landmark/landmark_corner_2D.cpp
-src/landmark/landmark_container.cpp
-src/landmark/landmark_line_2D.cpp
-src/landmark/landmark_polyline_2D.cpp
-    )
-  SET(SRCS_PROCESSOR
-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
-src/processor/processor_odom_3D.cpp
-src/processor/processor_tracker_feature.cpp
-src/processor/processor_tracker_feature_dummy.cpp
-src/processor/processor_tracker_landmark_dummy.cpp
-src/processor/processor_tracker_landmark.cpp
-    )
-  SET(SRCS_SENSOR
-src/sensor/sensor_camera.cpp
-src/sensor/sensor_diff_drive.cpp
-src/sensor/sensor_GPS.cpp
-src/sensor/sensor_GPS_fix.cpp
-src/sensor/sensor_IMU.cpp
-src/sensor/sensor_odom_2D.cpp
-src/sensor/sensor_odom_3D.cpp
-    )
 SET(SRCS_DTASSC
-src/association/association_solver.cpp
-src/association/association_node.cpp
-src/association/association_tree.cpp
-src/association/association_nnls.cpp
-    )
+  src/association/association_solver.cpp
+  src/association/association_node.cpp
+  src/association/association_tree.cpp
+  src/association/association_nnls.cpp
+  )
 SET(SRCS_SOLVER
-src/solver/solver_manager.cpp
-)
+  src/solver/solver_manager.cpp
+  )
 #OPTIONALS
 #optional HDRS and SRCS
 IF (Ceres_FOUND)
     SET(HDRS_WRAPPER
-include/base/solver_suitesparse/sparse_utils.h
-include/base/solver/solver_manager.h
-include/base/ceres_wrapper/ceres_manager.h
-        #ceres_wrapper/qr_manager.h
-include/base/ceres_wrapper/cost_function_wrapper.h
-include/base/ceres_wrapper/create_numeric_diff_cost_function.h
-include/base/ceres_wrapper/local_parametrization_wrapper.h 
-        )
+      include/base/solver_suitesparse/sparse_utils.h
+      include/base/solver/solver_manager.h
+      include/base/ceres_wrapper/ceres_manager.h
+      #ceres_wrapper/qr_manager.h
+      include/base/ceres_wrapper/cost_function_wrapper.h
+      include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+      include/base/ceres_wrapper/local_parametrization_wrapper.h 
+      )
     SET(SRCS_WRAPPER
-src/solver/solver_manager.cpp
-src/ceres_wrapper/ceres_manager.cpp
-        #ceres_wrapper/qr_manager.cpp
-src/ceres_wrapper/local_parametrization_wrapper.cpp 
-        )
+      src/solver/solver_manager.cpp
+      src/ceres_wrapper/ceres_manager.cpp
+      #ceres_wrapper/qr_manager.cpp
+      src/ceres_wrapper/local_parametrization_wrapper.cpp 
+      )
 ELSE(Ceres_FOUND)
-    SET(HDRS_WRAPPER)
-    SET(SRCS_WRAPPER)
+  SET(HDRS_WRAPPER)
+  SET(SRCS_WRAPPER)
 ENDIF(Ceres_FOUND)
 
 IF (laser_scan_utils_FOUND)
-      SET(HDRS_CAPTURE ${HDRS_CAPTURE}
-        include/base/capture/capture_laser_2D.h
-        )
-    SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-      include/base/processor/processor_tracker_feature_corner.h
-      include/base/processor/processor_tracker_landmark_corner.h
-      include/base/processor/processor_tracker_landmark_polyline.h
-      )
-    SET(HDRS_SENSOR ${HDRS_SENSOR}
-      include/base/sensor/sensor_laser_2D.h
-      )
-    SET(SRCS ${SRCS}
-src/sensor/sensor_laser_2D.cpp
-src/processor/processor_tracker_feature_corner.cpp
-src/processor/processor_tracker_landmark_corner.cpp
-src/processor/processor_tracker_landmark_polyline.cpp
-        )
+  SET(HDRS_CAPTURE ${HDRS_CAPTURE}
+    include/base/capture/capture_laser_2D.h
+    )
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    include/base/processor/processor_tracker_feature_corner.h
+    include/base/processor/processor_tracker_landmark_corner.h
+    include/base/processor/processor_tracker_landmark_polyline.h
+    )
+  SET(HDRS_SENSOR ${HDRS_SENSOR}
+    include/base/sensor/sensor_laser_2D.h
+    )
+  SET(SRCS ${SRCS}
+    src/sensor/sensor_laser_2D.cpp
+    src/processor/processor_tracker_feature_corner.cpp
+    src/processor/processor_tracker_landmark_corner.cpp
+    src/processor/processor_tracker_landmark_polyline.cpp
+    )
 ENDIF(laser_scan_utils_FOUND)
 
 IF (raw_gps_utils_FOUND)
-    SET(HDRS_CAPTURE ${HDRS_CAPTURE}
-      include/base/capture/capture_GPS.h
-      )
-    SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-      include/base/processor/processor_GPS.h
-      )
-    SET(SRCS ${SRCS}
-src/capture/capture_GPS.cpp
-src/processor/processor_GPS.cpp
-        )
+  SET(HDRS_CAPTURE ${HDRS_CAPTURE}
+    include/base/capture/capture_GPS.h
+    )
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    include/base/processor/processor_GPS.h
+    )
+  SET(SRCS ${SRCS}
+    src/capture/capture_GPS.cpp
+    src/processor/processor_GPS.cpp
+    )
 ENDIF(raw_gps_utils_FOUND)
 
 # Vision
 IF (vision_utils_FOUND)
-    SET(HDRS_CAPTURE ${HDRS_CAPTURE}
-      include/base/capture/capture_image.h
+  SET(HDRS_CAPTURE ${HDRS_CAPTURE}
+    include/base/capture/capture_image.h
     )
   SET(HDRS_FEATURE ${HDRS_FEATURE}
     include/base/feature/feature_point_image.h
     )
-    SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-include/base/processor/processor_tracker_feature_trifocal.h
-include/base/processor/processor_params_image.h
-include/base/processor/processor_tracker_feature_image.h
-include/base/processor/processor_tracker_landmark_image.h
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    include/base/processor/processor_tracker_feature_trifocal.h
+    include/base/processor/processor_params_image.h
+    include/base/processor/processor_tracker_feature_image.h
+    include/base/processor/processor_tracker_landmark_image.h
     )
   SET(HDRS_LANDMARK ${HDRS_LANDMARK}
-include/base/landmark/landmark_point_3D.h
-include/base/landmark/landmark_AHP.h
+    include/base/landmark/landmark_point_3D.h
+    include/base/landmark/landmark_AHP.h
     )
-    SET(SRCS ${SRCS}
-src/capture/capture_image.cpp
-src/feature/feature_point_image.cpp
-        )
-    SET(SRCS_LANDMARK ${SRCS_LANDMARK}
-src/landmark/landmark_point_3D.cpp
-src/landmark/landmark_AHP.cpp
+  SET(SRCS ${SRCS}
+    src/capture/capture_image.cpp
+    src/feature/feature_point_image.cpp
     )
-    SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-src/processor/processor_tracker_feature_trifocal.cpp
-src/processor/processor_tracker_feature_image.cpp
-src/processor/processor_tracker_landmark_image.cpp
+  SET(SRCS_LANDMARK ${SRCS_LANDMARK}
+    src/landmark/landmark_point_3D.cpp
+    src/landmark/landmark_AHP.cpp
+    )
+  SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    src/processor/processor_tracker_feature_trifocal.cpp
+    src/processor/processor_tracker_feature_image.cpp
+    src/processor/processor_tracker_landmark_image.cpp
     )
 ENDIF(vision_utils_FOUND)
-
 #SUBDIRECTORIES
 add_subdirectory(hello_wolf)
 IF (cereal_FOUND)
@@ -668,56 +667,56 @@ IF (cereal_FOUND)
 ENDIF(cereal_FOUND)
 
 IF (Suitesparse_FOUND)
-    #DOES NOTHING?!    
-    #ADD_SUBDIRECTORY(solver_suitesparse)
+  #DOES NOTHING?!    
+  #ADD_SUBDIRECTORY(solver_suitesparse)
 ENDIF(Suitesparse_FOUND)
 # LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
 IF(YAMLCPP_FOUND)
-    # headers
-    SET(HDRS ${HDRS}
-include/base/yaml/yaml_conversion.h
-        )
-    SET(HDRS_YAML ${HDRS_YAML}
-include/base/yaml/yaml_conversion.h
-        )
-
-    # sources
+  # headers
+  SET(HDRS ${HDRS}
+    include/base/yaml/yaml_conversion.h
+    )
+  SET(HDRS_YAML ${HDRS_YAML}
+    include/base/yaml/yaml_conversion.h
+    )
+  
+  # sources
+  SET(SRCS ${SRCS}
+    src/yaml/processor_odom_3D_yaml.cpp
+    src/yaml/processor_IMU_yaml.cpp
+    src/yaml/sensor_camera_yaml.cpp
+    src/yaml/sensor_odom_3D_yaml.cpp
+    src/yaml/sensor_IMU_yaml.cpp
+    )
+  IF(laser_scan_utils_FOUND)
     SET(SRCS ${SRCS}
-src/yaml/processor_odom_3D_yaml.cpp
-src/yaml/processor_IMU_yaml.cpp
-src/yaml/sensor_camera_yaml.cpp
-src/yaml/sensor_odom_3D_yaml.cpp
-src/yaml/sensor_IMU_yaml.cpp
-        )
-    IF(laser_scan_utils_FOUND)
-        SET(SRCS ${SRCS}
-src/yaml/sensor_laser_2D_yaml.cpp
-            )
-    ENDIF(laser_scan_utils_FOUND)
-    IF(vision_utils_FOUND)
-        SET(SRCS ${SRCS}
-src/yaml/processor_image_yaml.cpp
-src/yaml/processor_tracker_feature_trifocal_yaml.cpp
-            )
-    ENDIF(vision_utils_FOUND)
+      src/yaml/sensor_laser_2D_yaml.cpp
+      )
+  ENDIF(laser_scan_utils_FOUND)
+  IF(vision_utils_FOUND)
+    SET(SRCS ${SRCS}
+      src/yaml/processor_image_yaml.cpp
+      src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+      )
+  ENDIF(vision_utils_FOUND)
 ENDIF(YAMLCPP_FOUND)
 
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} 
-            SHARED 
-            ${SRCS_BASE} 
-            ${SRCS_CORE}
-            ${SRCS}
-            ${SRCS_CAPTURE}
-            ${SRCS_CONSTRAINT}
-            ${SRCS_FEATURE}
-            ${SRCS_LANDMARK}
-            ${SRCS_PROCESSOR}
-            ${SRCS_SENSOR}
-            #${SRCS_DTASSC} 
-            ${SRCS_SOLVER}
-            ${SRCS_WRAPPER}
-            )
+  SHARED 
+  ${SRCS_BASE} 
+  ${SRCS_CORE}
+  ${SRCS}
+  ${SRCS_CAPTURE}
+  ${SRCS_CONSTRAINT}
+  ${SRCS_FEATURE}
+  ${SRCS_LANDMARK}
+  ${SRCS_PROCESSOR}
+  ${SRCS_SENSOR}
+  #${SRCS_DTASSC} 
+  ${SRCS_SOLVER}
+  ${SRCS_WRAPPER}
+  )
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 
 #Link the created libraries