diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53292bfc46bd6bf558c4b188b1beda77c6378be6..2a8831e5bade22ee5dcc2f1c6bd51194314436fb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,12 +57,12 @@ CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
 CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 if(COMPILER_SUPPORTS_CXX11)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 elseif(COMPILER_SUPPORTS_CXX0X)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
 else()
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 
 
@@ -124,17 +124,17 @@ FIND_PACKAGE(faramotics QUIET) #faramotics is not required
 IF(faramotics_FOUND)
 	FIND_PACKAGE(GLUT REQUIRED)
 	FIND_PACKAGE(pose_state_time REQUIRED)
-    MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
+  MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
 ENDIF(faramotics_FOUND)
 
 FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
 IF(laser_scan_utils_FOUND)
-    MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
+  MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
 ENDIF(laser_scan_utils_FOUND)
 
 FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
 IF(raw_gps_utils_FOUND)
-    MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
+  MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
 ENDIF(raw_gps_utils_FOUND)
 
 # Vision Utils
@@ -148,7 +148,7 @@ ENDIF(vision_utils_FOUND)
 # Cereal
 FIND_PACKAGE(cereal QUIET)
 IF(cereal_FOUND)
-    MESSAGE("cereal Library FOUND: cereal related sources will be built.")
+  MESSAGE("cereal Library FOUND: cereal related sources will be built.")
 ENDIF(cereal_FOUND)
 
 # Apriltag
@@ -268,427 +268,427 @@ ENDIF(APRILTAG_INCLUDE_DIR)
 #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 
-        )
-    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 
-        )
+  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 
+    )
+  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 
+    )
 ELSE(Ceres_FOUND)
     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)
 
 IF (OPENCV_FOUND AND Apriltag_FOUND)
-    SET(HDRS ${HDRS}
-      include/base/landmark/landmark_apriltag.h
-      include/base/feature/feature_apriltag.h
-      include/base/processor/processor_tracker_landmark_apriltag.h
-      include/base/processor/ippe.h
-      include/base/constraint/constraint_autodiff_apriltag.h
-        )
-    SET(SRCS ${SRCS}
-      src/landmark/landmark_apriltag.cpp
-      src/feature/feature_apriltag.cpp
-      src/processor/processor_tracker_landmark_apriltag.cpp
-      src/processor/ippe.cpp
-        )
+  SET(HDRS ${HDRS}
+    include/base/landmark/landmark_apriltag.h
+    include/base/feature/feature_apriltag.h
+    include/base/processor/processor_tracker_landmark_apriltag.h
+    include/base/processor/ippe.h
+    include/base/constraint/constraint_autodiff_apriltag.h
+    )
+  SET(SRCS ${SRCS}
+    src/landmark/landmark_apriltag.cpp
+    src/feature/feature_apriltag.cpp
+    src/processor/processor_tracker_landmark_apriltag.cpp
+    src/processor/ippe.cpp
+    )
 ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
 
 #SUBDIRECTORIES
@@ -703,56 +703,56 @@ IF (Suitesparse_FOUND)
 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
-        )
+  # headers
+  SET(HDRS ${HDRS}
+    include/base/yaml/yaml_conversion.h
+    )
+  SET(HDRS_YAML ${HDRS_YAML}
+    include/base/yaml/yaml_conversion.h
+    )
 
-    # sources
+  # 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/sensor_laser_2D_yaml.cpp
+      )
+  ENDIF(laser_scan_utils_FOUND)
+  IF(vision_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
+      src/yaml/processor_image_yaml.cpp
+      src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+      )
+    IF(Apriltag_FOUND)
+      SET(SRCS ${SRCS}
+        src/yaml/processor_tracker_landmark_apriltag_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
-            )
-            IF(Apriltag_FOUND)
-                SET(SRCS ${SRCS}
-                  src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
-                )
-            ENDIF(Apriltag_FOUND)
-    ENDIF(vision_utils_FOUND)
+    ENDIF(Apriltag_FOUND)
+  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