diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5f29c8af3cfda3da6eedefa90f8bfdeb86c04e2c..080f8ed17e1c30f754d3307ae77beaa9e863877f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,19 +1,6 @@
 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-# Include gtest directory.
-# if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-#   message("CMake version less than 3.11.0")
-#   include_directories(${GTEST_INCLUDE_DIRS})
-# endif()
-
-############# USE THIS TEST AS AN EXAMPLE #################
-#                                                         #
-# Create a specific test executable for gtest_example     #
-wolf_add_gtest(gtest_example gtest_example.cpp)           #
-target_link_libraries(gtest_example ${PLUGIN_NAME})      #
-#                                                         #
-##########################################################
 set(SRC_DUMMY
   dummy/processor_tracker_feature_dummy.cpp
   dummy/processor_tracker_landmark_dummy.cpp
@@ -21,274 +8,217 @@ set(SRC_DUMMY
   )
 add_library(dummy SHARED ${SRC_DUMMY})
 target_link_libraries(dummy ${PLUGIN_NAME})
+
+############# USE THIS TEST AS AN EXAMPLE #################
+#                                                         #
+# Create a specific test executable for gtest_example     #
+wolf_add_gtest(gtest_example gtest_example.cpp)           #
+#                                                         #
+###########################################################
+
 ################ ADD YOUR TESTS BELOW ####################
 #                                                        # 
 #           ==== IN ALPHABETICAL ORDER! ====             #
 #                                                        #
+##########################################################
 
 # ------- First Core classes ----------
 
 # BufferFrame
 wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
-target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
 
 # CaptureBase class test
 wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
 
 # Converter from String to various types used by the parameters server
 wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PLUGIN_NAME})
 
 # FactorBase class test
 wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
-target_link_libraries(gtest_factor_base ${PLUGIN_NAME})
 
 # FactorAutodiff class test
 wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
 
 # Factory test
 wolf_add_gtest(gtest_factory gtest_factory.cpp)
-target_link_libraries(gtest_factory ${PLUGIN_NAME} dummy)
 
 # FactoryStateBlock factory test
 wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
-target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
 
 # Node Emplace test
 wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
 
 # FeatureBase classes test
 wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
 
 # FrameBase classes test
 wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
 
 # GraphSearch class test
 wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
-target_link_libraries(gtest_graph_search ${PLUGIN_NAME})
 
 # HasStateBlocks classes test
 wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
-target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
 
 # IsMotion classes test
 wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
-target_link_libraries(gtest_motion_provider ${PLUGIN_NAME})
 
 # LocalParametrizationXxx classes test
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PLUGIN_NAME})
 
 # Logging test
 wolf_add_gtest(gtest_logging gtest_logging.cpp)
-target_link_libraries(gtest_logging ${PLUGIN_NAME})
 
 # MotionBuffer class test
 wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
 
 # Parameters server
 wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
-target_link_libraries(gtest_param_server ${PLUGIN_NAME})
 
 # YAML parser
 wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME})
 
 # Problem class test
 wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
 
 # ProcessorBase class test
 wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
 
 # ProcessorMotion class test
 wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
   
 # Rotation test
 wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-target_link_libraries(gtest_rotation ${PLUGIN_NAME})
 
 # SE3 test
 wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
-target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
 
 # SE2 test
 wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
-target_link_libraries(gtest_SE2 ${PLUGIN_NAME})
 
 # SensorBase test
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})
 
 # shared_from_this test
 wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PLUGIN_NAME})
 
 # SolverManager test
 wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-target_link_libraries(gtest_solver_manager ${PLUGIN_NAME})
 
 # SolverManagerMultithread test
 wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp)
-target_link_libraries(gtest_solver_manager_multithread ${PLUGIN_NAME})
 
 # StateBlock class test
 wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
-target_link_libraries(gtest_state_block ${PLUGIN_NAME})
 
 # StateBlock class test
 wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
-target_link_libraries(gtest_state_composite ${PLUGIN_NAME})
 
 # TimeStamp class test
 wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
-target_link_libraries(gtest_time_stamp ${PLUGIN_NAME})
 
 # TrackMatrix class test
 wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PLUGIN_NAME})
 
 # TrajectoryBase class test
 wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-target_link_libraries(gtest_trajectory ${PLUGIN_NAME})
 
 # TreeManager class test
 wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
-target_link_libraries(gtest_tree_manager ${PLUGIN_NAME})
 
 # ------- Now Derived classes ----------
 
 # FactorAbs(P/O/V) classes test
 wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-target_link_libraries(gtest_factor_absolute ${PLUGIN_NAME})
 
 # FactorAutodiffDistance3d test
 wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
-target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME})
 
 # FactorBlockDifference class test
 wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
-target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
 
 # FactorDiffDrive class test
 wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
-target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
 
 # FactorOdom2dAutodiff class test
 wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
-target_link_libraries(gtest_factor_odom_2d_autodiff ${PLUGIN_NAME})
 
 # FactorPose2d class test
 wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
-target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
 
 # FactorPose3d class test
 wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
-target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
 
 # FactorRelativePose2d class test
 wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_NAME})
 
 # FactorRelativePose2dWithExtrinsics class test
 wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME})
 
 # FactorRelativePose3d class test
 wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
-target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
 
 # FactorRelativePose3dWithExtrinsics class test
 wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
 
 # FactorVelocityLocalDirection3d class test
 wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
-target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
 
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
 
 # Map yaml save/load test
 wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
-target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
 
 # Parameter prior test
 wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
 
 # ProcessorFixedWingModel class test
 wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
-target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
 
 # ProcessorDiffDrive class test
 wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
-target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
 
 # ProcessorLoopClosure class test
 wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
-target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
 
 # ProcessorFrameNearestNeighborFilter class test
 # wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
-# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME})
 
 # ProcessorMotion in 2d
 wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
 
 # ProcessorOdom3d class test
 wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
 
 # FactorPose3dWithExtrinsics class test
 wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
-
 
 # ProcessorTrackerFeatureDummy class test
 wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
-target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy)
 
 # ProcessorTrackerLandmarkDummy class test
 wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
-target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dummy)
 
 # SensorDiffDriveSelfcalib class test
 wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
-target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
 
 # SensorDiffDriveSelfcalib class test
 wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
-target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
 
 IF (Ceres_FOUND)
 	# SolverCeres test
 	wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
-	target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME})
 	
 	# SolverCeresMultithread test
 	wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
-	target_link_libraries(gtest_solver_ceres_multithread ${PLUGIN_NAME})
 ENDIF(Ceres_FOUND)
 
 # TreeManagerSlidingWindow class test
 wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
-target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME})
 
 # TreeManagerSlidingWindowDualRate class test
 wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
-target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME})
 
 # yaml conversions
-wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
-target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME})
+wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
\ No newline at end of file
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 254e09cb51609cd33408567e68ee1fc2035ea8a6..9e2a84991e411e226741ffa40b192ce5eafd3460 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -71,10 +71,10 @@ function(wolf_add_gtest target)
   add_executable(${target} ${ARGN})
   if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
     add_dependencies(${target} libgtest)
-    target_link_libraries(${target} libgtest)
+    target_link_libraries(${target} libgtest ${PLUGIN_NAME} dummy)
     target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
   else()
-    target_link_libraries(${target} PUBLIC gtest_main)
+    target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME} dummy)
   endif()
   add_test(NAME ${target} COMMAND ${target})
 endfunction()