diff --git a/.gitignore b/.gitignore
index 63651704510524003d8c5eb988447d63b842aca0..2a19e53e62aaa9c251b2f92657392e589597f1d6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,8 +6,8 @@ bin/
 build/
 build_debug/
 build_release/
-build-release/
 build-debug/
+build-release/
 lib/
 .idea/
 ./Wolf.user
@@ -33,7 +33,6 @@ src/CMakeFiles/cmake.check_cache
 src/examples/map_apriltag_save.yaml
 
 \.vscode/
-build_release/
 .clangd
 wolfcore.found
 /wolf.found
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 92aa03bb2bdb38b56f8122aab2818d9beb9779c8..8337cb1148e9f8669aecc9a6ab7558c5a08446b9 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -18,6 +18,7 @@ stages:
   - echo $GNSSUTILS_BRANCH
   - echo $LASERSCANUTILS_BRANCH
   - echo $DEPLOY_CI_ROS
+  - echo $WOLF_ROS_NODE_BRANCH
   - echo $WOLF_ROS_IMU_BRANCH
   - echo $WOLF_ROS_GNSS_BRANCH
   - echo $WOLF_ROS_LASER_BRANCH
@@ -299,9 +300,11 @@ deploy_wolf_ros_node:
     WOLF_ROS_VISION_BRANCH: $WOLF_ROS_VISION_BRANCH
     WOLF_ROS_APRILTAG_BRANCH: $WOLF_ROS_APRILTAG_BRANCH
     WOLF_ROS_BODYDYNAMICS_BRANCH: $WOLF_ROS_BODYDYNAMICS_BRANCH
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
-    branch: $WOLF_ROS_CORE_BRANCH
+    branch: $WOLF_ROS_NODE_BRANCH
 
 ############ DEPLOY WOLF_ROS_NODE FOR main ############
 deploy_wolf_ros_node_main:
@@ -322,6 +325,8 @@ deploy_wolf_ros_node_main:
     WOLF_ROS_VISION_BRANCH: main
     WOLF_ROS_APRILTAG_BRANCH: main
     WOLF_ROS_BODYDYNAMICS_BRANCH: main
+    GNSSUTILS_BRANCH: main
+    LASERSCANUTILS_BRANCH: main
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
     branch: main
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c94b33bfb04c9751c727f62bb16557cd1ef8f508..e69a3926a1a15adc529a45d8c37c7aa3a5437ff5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,12 +2,7 @@
 MESSAGE(STATUS "Starting WOLF CMakeLists ...")
 
 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-if(COMMAND cmake_policy)
-  cmake_policy(SET CMP0005 NEW)
-  cmake_policy(SET CMP0003 NEW)
-endif(COMMAND cmake_policy)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH 1)
@@ -17,10 +12,11 @@ PROJECT(core)
 set(PLUGIN_NAME "wolf${PROJECT_NAME}")
 
 # Paths
+SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-SET(CMAKE_INSTALL_PREFIX /usr/local)
-SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
+SET(LIB_INSTALL_DIR lib)
+SET(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
 
 # Build type
 IF (NOT CMAKE_BUILD_TYPE)
@@ -80,21 +76,18 @@ ENDIF()
 
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
-#CMAKE modules
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH})
-
 # ============ DEPENDENCIES ============ 
-FIND_PACKAGE(Threads REQUIRED)
-FIND_PACKAGE(Ceres REQUIRED)
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+FIND_PACKAGE(Threads REQUIRED MODULE)
+FIND_PACKAGE(Ceres REQUIRED CONFIG)
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
 if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
   message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
 endif()
-FIND_PACKAGE(YamlCpp REQUIRED)
+FIND_PACKAGE(yaml-cpp REQUIRED CONFIG)
 
 # ============ config.h ============ 
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+set(_WOLF_LIB_DIR ${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR})
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
 
@@ -124,9 +117,7 @@ ENDIF (SPDLOG_INCLUDE_DIR)
 
 # ============ INCLUDES ============ 
 INCLUDE_DIRECTORIES("include") # In this same project
-INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
+
 # ============ HEADERS ============ 
 SET(HDRS_CAPTURE
   include/core/capture/capture_base.h
@@ -382,10 +373,6 @@ ELSE(Ceres_FOUND)
 	SET(SRCS_CERES_WRAPPER)
 ENDIF(Ceres_FOUND)
 
-IF (Suitesparse_FOUND)
-  #ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
 # create the shared library
 ADD_LIBRARY(${PLUGIN_NAME}
   SHARED
@@ -423,21 +410,22 @@ endif()
 
 #Link the created libraries
 #=============================================================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ${CMAKE_THREAD_LIBS_INIT} dl)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC yaml-cpp)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
 IF (Ceres_FOUND)
-    TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
+    TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ceres)
 ENDIF(Ceres_FOUND)
 
 #Build tests
-#===============EXAMPLE=========================
+#========================================
 IF(BUILD_TESTS)
   MESSAGE(STATUS "Will build tests.")
   add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
 #Build demos
-#===============EXAMPLE=========================
+#========================================
 IF(BUILD_DEMOS)
   MESSAGE(STATUS "Will build demos.")
   ADD_SUBDIRECTORY(demos)
@@ -447,73 +435,85 @@ ENDIF(BUILD_DEMOS)
 #=============================================================
 INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
   RUNTIME DESTINATION bin
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib)
+  LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+  ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+
+
+# Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+  ${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+  PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+  FILES 
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  DESTINATION 
+  ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+)
+
+# Specifies include directories to use when compiling the plugin target
+# This way, include_directories does not need to be called in plugins depending on this one
+target_include_directories(${PLUGIN_NAME} INTERFACE
+  $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
 
-install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
 #install headers
 INSTALL(FILES ${HDRS_CAPTURE}
-   DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
 INSTALL(FILES ${HDRS_CERES_WRAPPER}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/ceres_wrapper)
 INSTALL(FILES ${HDRS_COMMON}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/common)
 INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
 INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
 INSTALL(FILES ${HDRS_FRAME}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/frame)
 INSTALL(FILES ${HDRS_HARDWARE}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/hardware)
 INSTALL(FILES ${HDRS_LANDMARK}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/landmark)
 INSTALL(FILES ${HDRS_MAP}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/map)
 INSTALL(FILES ${HDRS_MATH}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
 INSTALL(FILES ${HDRS_PROBLEM}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/problem)
 INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
 INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
 INSTALL(FILES ${HDRS_SOLVER}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver)
 #INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-#  DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
+#  DESTINATION ${INCLUDE_INSTALL_DIR}/core/solver_suitesparse)
 INSTALL(FILES ${HDRS_STATE_BLOCK}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
 INSTALL(FILES ${HDRS_TRAJECTORY}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/trajectory)
 INSTALL(FILES ${HDRS_TREE_MANAGER}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
 INSTALL(FILES ${HDRS_UTILS}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
 INSTALL(FILES ${HDRS_YAML}
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml)
 
-FILE(WRITE ${PLUGIN_NAME}.found "")
-INSTALL(FILES ${PLUGIN_NAME}.found
-  DESTINATION include/iri-algorithms/wolf/plugin_core)
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-  DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
-               "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" @ONLY)
-
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
-               "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
-INSTALL(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
-INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
 
 INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 
 export(PACKAGE ${PLUGIN_NAME})
 
-FIND_PACKAGE(Doxygen)
+FIND_PACKAGE(Doxygen MODULE)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
 IF (IRI_DOC_DIR)
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
deleted file mode 100644
index 9e969786089ca8ea3801be8b084c51a5782f09b5..0000000000000000000000000000000000000000
--- a/cmake_modules/FindEigen3.cmake
+++ /dev/null
@@ -1,97 +0,0 @@
-# - Try to find Eigen3 lib
-#
-# This module supports requiring a minimum version, e.g. you can do
-#   find_package(Eigen3 3.1.2)
-# to require version 3.1.2 or newer of Eigen3.
-#
-# Once done this will define
-#
-#  EIGEN3_FOUND - system has eigen lib with correct version
-#  EIGEN3_INCLUDE_DIR - the eigen include directory
-#  EIGEN3_VERSION - eigen version
-#
-# This module reads hints about search locations from 
-# the following enviroment variables:
-#
-# EIGEN3_ROOT
-# EIGEN3_ROOT_DIR
-
-# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
-# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
-# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
-
-if(NOT Eigen3_FIND_VERSION)
-  if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 2)
-  endif(NOT Eigen3_FIND_VERSION_MAJOR)
-  if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 91)
-  endif(NOT Eigen3_FIND_VERSION_MINOR)
-  if(NOT Eigen3_FIND_VERSION_PATCH)
-    set(Eigen3_FIND_VERSION_PATCH 0)
-  endif(NOT Eigen3_FIND_VERSION_PATCH)
-
-  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
-endif(NOT Eigen3_FIND_VERSION)
-
-macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
-
-  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
-  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
-
-  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
-  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK FALSE)
-  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK TRUE)
-  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-
-  if(NOT EIGEN3_VERSION_OK)
-
-    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
-                   "but at least version ${Eigen3_FIND_VERSION} is required")
-  endif(NOT EIGEN3_VERSION_OK)
-endmacro(_eigen3_check_version)
-
-if (EIGEN3_INCLUDE_DIR)
-
-  # in cache already
-  _eigen3_check_version()
-  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
-
-else (EIGEN3_INCLUDE_DIR)
-  
-  # search first if an Eigen3Config.cmake is available in the system,
-  # if successful this would set EIGEN3_INCLUDE_DIR and the rest of
-  # the script will work as usual
-  find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
-
-  if(NOT EIGEN3_INCLUDE_DIR)
-    find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
-        HINTS
-        ENV EIGEN3_ROOT 
-        ENV EIGEN3_ROOT_DIR
-        PATHS
-        ${CMAKE_INSTALL_PREFIX}/include
-        ${KDE4_INCLUDE_DIR}
-        PATH_SUFFIXES eigen3 eigen
-      )
-  endif(NOT EIGEN3_INCLUDE_DIR)
-
-  if(EIGEN3_INCLUDE_DIR)
-    _eigen3_check_version()
-  endif(EIGEN3_INCLUDE_DIR)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
-
-  mark_as_advanced(EIGEN3_INCLUDE_DIR)
-
-endif(EIGEN3_INCLUDE_DIR)
-
diff --git a/cmake_modules/FindGlog.cmake b/cmake_modules/FindGlog.cmake
deleted file mode 100644
index 979dceda4b8ec5e1a5457f4e32dbdc9a27834a1e..0000000000000000000000000000000000000000
--- a/cmake_modules/FindGlog.cmake
+++ /dev/null
@@ -1,346 +0,0 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-#   this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-#   this list of conditions and the following disclaimer in the documentation
-#   and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-#   used to endorse or promote products derived from this software without
-#   specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: alexs.mac@gmail.com (Alex Stewart)
-#
-
-# FindGlog.cmake - Find Google glog logging library.
-#
-# This module defines the following variables:
-#
-# GLOG_FOUND: TRUE iff glog is found.
-# GLOG_INCLUDE_DIRS: Include directories for glog.
-# GLOG_LIBRARIES: Libraries required to link glog.
-# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found
-#                                           was built & installed / exported
-#                                           as a CMake package.
-#
-# The following variables control the behaviour of this module:
-#
-# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then
-#                           then prefer using an exported CMake configuration
-#                           generated by glog > 0.3.4 over searching for the
-#                           glog components manually.  Otherwise (FALSE)
-#                           ignore any exported glog CMake configurations and
-#                           always perform a manual search for the components.
-#                           Default: TRUE iff user does not define this variable
-#                           before we are called, and does NOT specify either
-#                           GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS
-#                           otherwise FALSE.
-# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to
-#                         search for glog includes, e.g: /timbuktu/include.
-# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to
-#                         search for glog libraries, e.g: /timbuktu/lib.
-#
-# The following variables are also defined by this module, but in line with
-# CMake recommended FindPackage() module style should NOT be referenced directly
-# by callers (use the plural variables detailed above instead).  These variables
-# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
-# are NOT re-called (i.e. search for library is not repeated) if these variables
-# are set with valid values _in the CMake cache_. This means that if these
-# variables are set directly in the cache, either by the user in the CMake GUI,
-# or by the user passing -DVAR=VALUE directives to CMake when called (which
-# explicitly defines a cache variable), then they will be used verbatim,
-# bypassing the HINTS variables and other hard-coded search locations.
-#
-# GLOG_INCLUDE_DIR: Include directory for glog, not including the
-#                   include directory of any dependencies.
-# GLOG_LIBRARY: glog library, not including the libraries of any
-#               dependencies.
-
-# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
-# FindGlog was invoked.
-macro(GLOG_RESET_FIND_LIBRARY_PREFIX)
-  if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES)
-    set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
-  endif()
-endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)
-
-# Called if we failed to find glog or any of it's required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
-macro(GLOG_REPORT_NOT_FOUND REASON_MSG)
-  unset(GLOG_FOUND)
-  unset(GLOG_INCLUDE_DIRS)
-  unset(GLOG_LIBRARIES)
-  # Make results of search visible in the CMake GUI if glog has not
-  # been found so that user does not have to toggle to advanced view.
-  mark_as_advanced(CLEAR GLOG_INCLUDE_DIR
-                         GLOG_LIBRARY)
-
-  glog_reset_find_library_prefix()
-
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
-  # use the camelcase library name, not uppercase.
-  if (Glog_FIND_QUIETLY)
-    message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN})
-  elseif (Glog_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use no priority which emits a message
-    # but continues configuration and allows generation.
-    message("-- Failed to find glog - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(GLOG_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set GLOG_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(GLOG_FOUND)
-
-# -----------------------------------------------------------------
-# By default, if the user has expressed no preference for using an exported
-# glog CMake configuration over performing a search for the installed
-# components, and has not specified any hints for the search locations, then
-# prefer a glog exported configuration if available.
-if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION
-    AND NOT GLOG_INCLUDE_DIR_HINTS
-    AND NOT GLOG_LIBRARY_DIR_HINTS)
-  message(STATUS "No preference for use of exported glog CMake configuration "
-    "set, and no hints for include/library directories provided. "
-    "Defaulting to preferring an installed/exported glog CMake configuration "
-    "if available.")
-  set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE)
-endif()
-
-if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
-  # Try to find an exported CMake configuration for glog, as generated by
-  # glog versions > 0.3.4
-  #
-  # We search twice, s/t we can invert the ordering of precedence used by
-  # find_package() for exported package build directories, and installed
-  # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7)
-  # respectively in [1].
-  #
-  # By default, exported build directories are (in theory) detected first, and
-  # this is usually the case on Windows.  However, on OS X & Linux, the install
-  # path (/usr/local) is typically present in the PATH environment variable
-  # which is checked in item 4) in [1] (i.e. before both of the above, unless
-  # NO_SYSTEM_ENVIRONMENT_PATH is passed).  As such on those OSs installed
-  # packages are usually detected in preference to exported package build
-  # directories.
-  #
-  # To ensure a more consistent response across all OSs, and as users usually
-  # want to prefer an installed version of a package over a locally built one
-  # where both exist (esp. as the exported build directory might be removed
-  # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which
-  # means any build directories exported by the user are ignored, and thus
-  # installed directories are preferred.  If this fails to find the package
-  # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any
-  # exported build directories will now be detected.
-  #
-  # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which
-  # is item 5) in [1]), to not preferentially use projects that were built
-  # recently with the CMake GUI to ensure that we always prefer an installed
-  # version if available.
-  #
-  # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its
-  #       project name when built with CMake, but exports itself as just 'glog'.
-  #       On Linux/OS X this does not break detection as the project name is
-  #       not used as part of the install path for the CMake package files,
-  #       e.g. /usr/local/lib/cmake/glog, where the <glog> suffix is hardcoded
-  #       in glog's CMakeLists.  However, on Windows the project name *is*
-  #       part of the install prefix: C:/Program Files/google-glog/[include,lib].
-  #       However, by default CMake checks:
-  #       C:/Program Files/<FIND_PACKAGE_ARGUMENT_NAME='glog'> which does not
-  #       exist and thus detection fails.  Thus we use the NAMES to force the
-  #       search to use both google-glog & glog.
-  #
-  # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package
-  find_package(glog QUIET
-                    NAMES google-glog glog
-                    NO_MODULE
-                    NO_CMAKE_PACKAGE_REGISTRY
-                    NO_CMAKE_BUILDS_PATH)
-  if (glog_FOUND)
-    message(STATUS "Found installed version of glog: ${glog_DIR}")
-  else()
-    # Failed to find an installed version of glog, repeat search allowing
-    # exported build directories.
-    message(STATUS "Failed to find installed glog CMake configuration, "
-      "searching for glog build directories exported with CMake.")
-    # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and
-    # do not want to treat projects built with the CMake GUI preferentially.
-    find_package(glog QUIET
-                      NAMES google-glog glog
-                      NO_MODULE
-                      NO_CMAKE_BUILDS_PATH)
-    if (glog_FOUND)
-      message(STATUS "Found exported glog build directory: ${glog_DIR}")
-    endif(glog_FOUND)
-  endif(glog_FOUND)
-
-  set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND})
-
-  if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
-    message(STATUS "Detected glog version: ${glog_VERSION}")
-    set(GLOG_FOUND ${glog_FOUND})
-    # glog wraps the include directories into the exported glog::glog target.
-    set(GLOG_INCLUDE_DIR "")
-    set(GLOG_LIBRARY glog::glog)
-  else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
-    message(STATUS "Failed to find an installed/exported CMake configuration "
-      "for glog, will perform search for installed glog components.")
-  endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
-endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)
-
-if (NOT GLOG_FOUND)
-  # Either failed to find an exported glog CMake configuration, or user
-  # told us not to use one.  Perform a manual search for all glog components.
-
-  # Handle possible presence of lib prefix for libraries on MSVC, see
-  # also GLOG_RESET_FIND_LIBRARY_PREFIX().
-  if (MSVC)
-    # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES
-    # s/t we can set it back before returning.
-    set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}")
-    # The empty string in this list is important, it represents the case when
-    # the libraries have no prefix (shared libraries / DLLs).
-    set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}")
-  endif (MSVC)
-
-  # Search user-installed locations first, so that we prefer user installs
-  # to system installs where both exist.
-  list(APPEND GLOG_CHECK_INCLUDE_DIRS
-    /usr/local/include
-    /usr/local/homebrew/include # Mac OS X
-    /opt/local/var/macports/software # Mac OS X.
-    /opt/local/include
-    /usr/include)
-  # Windows (for C:/Program Files prefix).
-  list(APPEND GLOG_CHECK_PATH_SUFFIXES
-    glog/include
-    glog/Include
-    Glog/include
-    Glog/Include
-    google-glog/include # CMake installs with project name prefix.
-    google-glog/Include)
-
-  list(APPEND GLOG_CHECK_LIBRARY_DIRS
-    /usr/local/lib
-    /usr/local/homebrew/lib # Mac OS X.
-    /opt/local/lib
-    /usr/lib)
-  # Windows (for C:/Program Files prefix).
-  list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES
-    glog/lib
-    glog/Lib
-    Glog/lib
-    Glog/Lib
-    google-glog/lib # CMake installs with project name prefix.
-    google-glog/Lib)
-
-  # Search supplied hint directories first if supplied.
-  find_path(GLOG_INCLUDE_DIR
-    NAMES glog/logging.h
-    HINTS ${GLOG_INCLUDE_DIR_HINTS}
-    PATHS ${GLOG_CHECK_INCLUDE_DIRS}
-    PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})
-  if (NOT GLOG_INCLUDE_DIR OR
-      NOT EXISTS ${GLOG_INCLUDE_DIR})
-    glog_report_not_found(
-      "Could not find glog include directory, set GLOG_INCLUDE_DIR "
-      "to directory containing glog/logging.h")
-  endif (NOT GLOG_INCLUDE_DIR OR
-    NOT EXISTS ${GLOG_INCLUDE_DIR})
-
-  find_library(GLOG_LIBRARY NAMES glog
-    HINTS ${GLOG_LIBRARY_DIR_HINTS}
-    PATHS ${GLOG_CHECK_LIBRARY_DIRS}
-    PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})
-  if (NOT GLOG_LIBRARY OR
-      NOT EXISTS ${GLOG_LIBRARY})
-    glog_report_not_found(
-      "Could not find glog library, set GLOG_LIBRARY "
-      "to full path to libglog.")
-  endif (NOT GLOG_LIBRARY OR
-    NOT EXISTS ${GLOG_LIBRARY})
-
-  # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets
-  # if called.
-  set(GLOG_FOUND TRUE)
-
-  # Glog does not seem to provide any record of the version in its
-  # source tree, thus cannot extract version.
-
-  # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and
-  # thus FIND_[PATH/LIBRARY] are not called, but specified locations are
-  # invalid, otherwise we would report the library as found.
-  if (GLOG_INCLUDE_DIR AND
-      NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
-    glog_report_not_found(
-      "Caller defined GLOG_INCLUDE_DIR:"
-      " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.")
-  endif (GLOG_INCLUDE_DIR AND
-    NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
-  # TODO: This regex for glog library is pretty primitive, we use lowercase
-  #       for comparison to handle Windows using CamelCase library names, could
-  #       this check be better?
-  string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY)
-  if (GLOG_LIBRARY AND
-      NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
-    glog_report_not_found(
-      "Caller defined GLOG_LIBRARY: "
-      "${GLOG_LIBRARY} does not match glog.")
-  endif (GLOG_LIBRARY AND
-    NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
-
-  glog_reset_find_library_prefix()
-
-endif(NOT GLOG_FOUND)
-
-# Set standard CMake FindPackage variables if found.
-if (GLOG_FOUND)
-  set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
-  set(GLOG_LIBRARIES ${GLOG_LIBRARY})
-endif (GLOG_FOUND)
-
-# If we are using an exported CMake glog target, the include directories are
-# wrapped into the target itself, and do not have to be (and are not)
-# separately specified.  In which case, we should not add GLOG_INCLUDE_DIRS
-# to the list of required variables in order that glog be reported as found.
-if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)
-  set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES)
-else()
-  set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES)
-endif()
-
-# Handle REQUIRED / QUIET optional arguments.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Glog DEFAULT_MSG
-  ${GLOG_REQUIRED_VARIABLES})
-
-# Only mark internal variables as advanced if we found glog, otherwise
-# leave them visible in the standard GUI for the user to set manually.
-if (GLOG_FOUND)
-  mark_as_advanced(FORCE GLOG_INCLUDE_DIR
-                         GLOG_LIBRARY
-                         glog_DIR) # Autogenerated by find_package(glog)
-endif (GLOG_FOUND)
diff --git a/cmake_modules/FindYamlCpp.cmake b/cmake_modules/FindYamlCpp.cmake
deleted file mode 100644
index 196c4754e2787a9fa4f47eb8716fd2525bc84fd4..0000000000000000000000000000000000000000
--- a/cmake_modules/FindYamlCpp.cmake
+++ /dev/null
@@ -1,99 +0,0 @@
-# Locate yaml-cpp
-#
-# This module defines
-#  YAMLCPP_FOUND, if false, do not try to link to yaml-cpp
-#  YAMLCPP_LIBNAME, name of yaml library
-#  YAMLCPP_LIBRARY, where to find yaml-cpp
-#  YAMLCPP_LIBRARY_RELEASE, where to find Release or RelWithDebInfo yaml-cpp
-#  YAMLCPP_LIBRARY_DEBUG, where to find Debug yaml-cpp
-#  YAMLCPP_INCLUDE_DIR, where to find yaml.h
-#  YAMLCPP_LIBRARY_DIR, the directories to find YAMLCPP_LIBRARY
-#
-# By default, the dynamic libraries of yaml-cpp will be found. To find the static ones instead,
-# you must set the YAMLCPP_USE_STATIC_LIBS variable to TRUE before calling find_package(YamlCpp ...)
-
-# attempt to find static library first if this is set
-if(YAMLCPP_USE_STATIC_LIBS)
-    set(YAMLCPP_STATIC libyaml-cpp.a)
-    set(YAMLCPP_STATIC_DEBUG libyaml-cpp-dbg.a)
-endif()
-
-if(${CMAKE_SYSTEM_NAME} MATCHES "Windows")    ### Set Yaml libary name for Windows
-  set(YAMLCPP_LIBNAME "libyaml-cppmd" CACHE STRING "Name of YAML library")
-  set(YAMLCPP_LIBNAME optimized ${YAMLCPP_LIBNAME} debug ${YAMLCPP_LIBNAME}d)
-else()                      ### Set Yaml libary name for Unix, Linux, OS X, etc
-  set(YAMLCPP_LIBNAME "yaml-cpp" CACHE STRING "Name of YAML library")
-endif()
-
-# find the yaml-cpp include directory
-find_path(YAMLCPP_INCLUDE_DIR
-  NAMES yaml-cpp/yaml.h
-  PATH_SUFFIXES include
-  PATHS
-    ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/include
-    ~/Library/Frameworks/yaml-cpp/include/
-    /Library/Frameworks/yaml-cpp/include/
-    /usr/local/include/
-    /usr/include/
-    /sw/yaml-cpp/         # Fink
-    /opt/local/yaml-cpp/  # DarwinPorts
-    /opt/csw/yaml-cpp/    # Blastwave
-    /opt/yaml-cpp/)
-
-# find the release yaml-cpp library
-find_library(YAMLCPP_LIBRARY_RELEASE
-  NAMES ${YAMLCPP_STATIC} yaml-cpp libyaml-cppmd.lib
-  PATH_SUFFIXES lib64 lib Release RelWithDebInfo
-  PATHS
-    ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/
-    ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/build
-    ~/Library/Frameworks
-    /Library/Frameworks
-    /usr/local
-    /usr
-    /sw
-    /opt/local
-    /opt/csw
-    /opt)
-
-# find the debug yaml-cpp library
-find_library(YAMLCPP_LIBRARY_DEBUG
-  NAMES ${YAMLCPP_STATIC_DEBUG} yaml-cpp-dbg libyaml-cppmdd.lib
-  PATH_SUFFIXES lib64 lib Debug
-  PATHS
-    ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/
-    ${PROJECT_SOURCE_DIR}/dependencies/yaml-cpp-0.5.1/build
-    ~/Library/Frameworks
-    /Library/Frameworks
-    /usr/local
-    /usr
-    /sw
-    /opt/local
-    /opt/csw
-    /opt)
-
-# set library vars
-set(YAMLCPP_LIBRARY ${YAMLCPP_LIBRARY_RELEASE})
-if(CMAKE_BUILD_TYPE MATCHES Debug AND EXISTS ${YAMLCPP_LIBRARY_DEBUG})
-  set(YAMLCPP_LIBRARY ${YAMLCPP_LIBRARY_DEBUG})
-endif()
-
-get_filename_component(YAMLCPP_LIBRARY_RELEASE_DIR ${YAMLCPP_LIBRARY_RELEASE} PATH)
-get_filename_component(YAMLCPP_LIBRARY_DEBUG_DIR ${YAMLCPP_LIBRARY_DEBUG} PATH)
-set(YAMLCPP_LIBRARY_DIR ${YAMLCPP_LIBRARY_RELEASE_DIR} ${YAMLCPP_LIBRARY_DEBUG_DIR})
-
-# handle the QUIETLY and REQUIRED arguments and set YAMLCPP_FOUND to TRUE if all listed variables are TRUE
-include(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(YamlCpp DEFAULT_MSG
-  YAMLCPP_INCLUDE_DIR
-  YAMLCPP_LIBRARY
-  YAMLCPP_LIBRARY_DIR)
-mark_as_advanced(
-  YAMLCPP_INCLUDE_DIR
-  YAMLCPP_LIBRARY_DIR
-  YAMLCPP_LIBRARY
-  YAMLCPP_LIBRARY_RELEASE
-  YAMLCPP_LIBRARY_RELEASE_DIR
-  YAMLCPP_LIBRARY_DEBUG
-  YAMLCPP_LIBRARY_DEBUG_DIR)
-
diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
deleted file mode 100644
index 5204cd493ca0740f38d99aef942a7df32ad7f71d..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfcoreConfig.cmake
+++ /dev/null
@@ -1,104 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfcore_INCLUDE_DIRS
-    NAMES wolfcore.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-IF(wolfcore_INCLUDE_DIRS)
-  MESSAGE("Found wolf core include dirs: ${wolfcore_INCLUDE_DIRS}")
-ELSE(wolfcore_INCLUDE_DIRS)
-  MESSAGE("Couldn't find wolf core include dirs")
-ENDIF(wolfcore_INCLUDE_DIRS)
-
-FIND_LIBRARY(
-    wolfcore_LIBRARIES
-    NAMES libwolfcore.so libwolfcore.dylib
-    PATHS /usr/local/lib)
-IF(wolfcore_LIBRARIES)
-  MESSAGE("Found wolf core lib: ${wolfcore_LIBRARIES}")
-ELSE(wolfcore_LIBRARIES)
-  MESSAGE("Couldn't find wolf core lib")
-ENDIF(wolfcore_LIBRARIES)
-
-IF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
-   SET(wolfcore_FOUND TRUE)
- ELSE(wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
-   set(wolfcore_FOUND FALSE)
-ENDIF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
-
-IF (wolfcore_FOUND)
-   IF (NOT wolfcore_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf core: ${wolfcore_LIBRARIES}")
-   ENDIF (NOT wolfcore_FIND_QUIETLY)
-ELSE (wolfcore_FOUND)
-   IF (wolfcore_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf core")
-   ENDIF (wolfcore_FIND_REQUIRED)
-ENDIF (wolfcore_FOUND)
-
-
-macro(wolfcore_report_not_found REASON_MSG)
-  set(wolfcore_FOUND FALSE)
-  unset(wolfcore_INCLUDE_DIRS)
-  unset(wolfcore_LIBRARIES)
-
-  # Reset the CMake module path to its state when this script was called.
-  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
-  # FindPackage() use the camelcase library name, not uppercase.
-  if (wolfcore_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
-  elseif (wolfcore_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
-    # that prevents generation, but continues configuration.
-    message(SEND_ERROR "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolfcore_report_not_found)
-
-if(NOT wolfcore_FOUND)
-  wolfcore_report_not_found("Something went wrong while setting up wolf.")
-else (NOT wolfcore_FOUND)
-  # Set the include directories for wolf core (itself).
-  set(wolfcore_FOUND TRUE)
-
-  # Now we gather all the required dependencies for Wolf
-
-  get_filename_component(WOLFCORE_CURRENT_CONFIG_DIR
-    "${CMAKE_CURRENT_LIST_FILE}" PATH)
-
-  SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH})
-  LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLFCORE_CURRENT_CONFIG_DIR})
-
-  FIND_PACKAGE(Threads REQUIRED)
-  list(APPEND wolfcore_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
-
-  FIND_PACKAGE(Ceres REQUIRED)
-  list(APPEND wolfcore_INCLUDE_DIRS ${CERES_INCLUDE_DIRS})
-  list(APPEND wolfcore_LIBRARIES ${CERES_LIBRARIES})
-
-  # YAML with yaml-cpp
-  FIND_PACKAGE(YamlCpp REQUIRED)
-  list(APPEND wolfcore_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
-  list(APPEND wolfcore_LIBRARIES ${YAMLCPP_LIBRARY})
-
-  #Eigen
-  # FIND_PACKAGE(Eigen3 REQUIRED)
-  # list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-
-  if(NOT Eigen3_FOUND)
-    FIND_PACKAGE(Eigen3 REQUIRED)
-  endif()
-  if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
-    message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
-  endif()
-  list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-endif(NOT wolfcore_FOUND)
-SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfcore_INCLUDE_DIR ${wolfcore_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES 
-SET(wolfcore_LIBRARY ${wolfcore_LIBRARIES})
diff --git a/cmake_modules/wolfcoreConfig.cmake.in b/cmake_modules/wolfcoreConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..735adef5984b0989b98947873a15e22a910172b4
--- /dev/null
+++ b/cmake_modules/wolfcoreConfig.cmake.in
@@ -0,0 +1,14 @@
+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+@PACKAGE_INIT@
+
+# forwards the correct parameters given to FIND_DEPENDENCIES
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(Threads REQUIRED)
+FIND_DEPENDENCY(Ceres REQUIRED)
+FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
+FIND_DEPENDENCY(yaml-cpp REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 1e18985f9a047dea575af0852b24eac6ff6305bd..790ec306644c12e607a4657859bedc8e360284aa 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -322,14 +322,20 @@ class Factory
         Factory(const Factory&)         = delete;
         void operator=(Factory const&)  = delete;
     private:
-        Factory()  { }
+        Factory();
         ~Factory();
 };
 
 template<class TypeBase, typename... TypeInput>
-inline Factory<TypeBase, TypeInput...>::~Factory<TypeBase, TypeInput...>()
+inline Factory<TypeBase, TypeInput...>::Factory()
 {
-    std::cout << " Factory destructor  " << this->getClass() << std::endl;
+    //std::cout << " Factory constructor  " << this->getClass() << std::endl;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline Factory<TypeBase, TypeInput...>::~Factory()
+{
+    //std::cout << " Factory destructor  " << this->getClass() << std::endl;
 }
 
 template<class TypeBase, typename... TypeInput>
@@ -340,7 +346,7 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
         std::cout << std::setw(26) << std::left << get().getClass() << " <--  registered  " << _type << std::endl;
     else
         std::cout << std::setw(26) << std::left << get().getClass() << " X--   skipping   " << _type << ": already registered." << std::endl;
-
+    
     return reg;
 }
 
diff --git a/internal/config.h.in b/internal/config.h.in
index 7c0fe4756e443695c5450cfb46054e8b7cd107a7..8f0d62ec930e2db6e5f4bc874126b8e0787eb856 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -32,5 +32,6 @@
 #cmakedefine _WOLF_TRACE
 
 #define _WOLF_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_LIB_DIR "${_WOLF_LIB_DIR}"
 
 #endif /* WOLF_INTERNAL_CONFIG_H_ */
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 604770acc4629ba97125e1da097d6e5871083983..6fe2afb34530e550d8a361e56bcaae2c07938349 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -100,8 +100,11 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
 
     // Load plugins
     auto loaders = std::vector<std::shared_ptr<Loader>>();
-    std::string plugins_path;
-    try
+    std::string plugins_path = _WOLF_LIB_DIR;
+    if (plugins_path.back() != '/'){
+        plugins_path += '/';  // only works for UNIX systems
+    }
+    /*try
     {
         plugins_path = _server.getParam<std::string>("plugins_path");
     }
@@ -110,7 +113,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
       WOLF_WARN(e.what());
       WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
       plugins_path="/usr/local/lib/";
-    }
+    }*/
     for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins"))
     {
       if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index dd7732c47dbffb1e1e7e2502d0ecc78c60e9f926..6fa50395a7ee2af50d290b32e543b18bac534869 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,36 +1,7 @@
 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-
-## Added these two include_dirs: ######################
-#
-#CMAKE modules
-#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-#
-# Include Eigen
-#FIND_PACKAGE(Eigen 3 REQUIRED)
-#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-#
-# Include Ceres
-#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-#IF(Ceres_FOUND)
-#    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-#ENDIF(Ceres_FOUND)
-#
-## and now gtest_motion_2d works ######################
-
-
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
-############# 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})      #
-#                                                         #
-###########################################################
+# Add here the source (.cpp) files in dummy folder
 set(SRC_DUMMY
   dummy/processor_tracker_feature_dummy.cpp
   dummy/processor_tracker_landmark_dummy.cpp
@@ -38,274 +9,230 @@ set(SRC_DUMMY
   )
 add_library(dummy SHARED ${SRC_DUMMY})
 target_link_libraries(dummy ${PLUGIN_NAME})
-################# ADD YOUR TESTS BELOW ####################
+
+############# USE THIS TEST AS AN EXAMPLE #################
+#                                                         #
+# Create a specific test executable for gtest_example     #
+wolf_add_gtest(gtest_example gtest_example.cpp)           #
 #                                                         #
-#           ==== IN ALPHABETICAL ORDER! ====              #
+# OPTIONAL: if the gtest depends on the dummy library     #
+# (used for implementing pure virtual base classes),      #
+# add a line                                              #
+# target_link_libraries(gtest_example dummy)       #
 #                                                         #
+###########################################################
 
-# ------- First Core classes ----------
+################ ADD YOUR TESTS BELOW ####################
+#                                                        # 
+#           ==== IN ALPHABETICAL ORDER! ====             #
+#                                                        #
+##########################################################
+
+# ------- First Base classes ----------
 
 # BufferFrame
 wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
-target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_buffer_frame PUBLIC 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)
+target_link_libraries(gtest_factory PUBLIC 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})
+target_link_libraries(gtest_factory_state_block PUBLIC dummy)
 
 # Node Emplace test
 wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_emplace PUBLIC 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})
+wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
 
 # 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)
+target_link_libraries(gtest_problem PUBLIC dummy)
 
 # ProcessorBase class test
 wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
+target_link_libraries(gtest_processor_base PUBLIC 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)
+target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC 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)
+target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC 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})
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..2294c819aec95c1f2c6bed5c0e7b1d6cc857c614 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,73 @@
-cmake_minimum_required(VERSION 2.8.8)
-project(gtest_builder C CXX)
+if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
+  message("CMake version less than 3.11.0")
 
-# We need thread support
-#find_package(Threads REQUIRED)
+  # Enable ExternalProject CMake module
+  include(ExternalProject)
 
-# Enable ExternalProject CMake module
-include(ExternalProject)
+  set(GTEST_FORCE_SHARED_CRT ON)
+  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
 
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+  # Download GoogleTest
+  ExternalProject_Add(googletest
+      GIT_REPOSITORY https://github.com/google/googletest.git
+      GIT_TAG        v1.8.x
+      # TIMEOUT 1 # We'll try this
+      CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+      -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+      -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+      -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+      -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+      -DBUILD_GTEST=ON
+      PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+      # Disable install step
+      INSTALL_COMMAND ""
+      UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+  )
 
-# For some reason I need to disable PTHREADS
-# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
-# This is a known issue for MinGW :
-# https://github.com/google/shaderc/pull/174
-#if(MINGW)
-    set(GTEST_DISABLE_PTHREADS ON)
-#endif()
+  # Get GTest source and binary directories from CMake project
 
-# Download GoogleTest
-ExternalProject_Add(googletest
-    GIT_REPOSITORY https://github.com/google/googletest.git
-    GIT_TAG        v1.8.x
-    # TIMEOUT 1 # We'll try this
-    CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
-    -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
-    -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
-    -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
-    -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
-    -DBUILD_GTEST=ON
-    PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
-    # Disable install step
-    INSTALL_COMMAND ""
-    UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-)
+  # Specify include dir
+  ExternalProject_Get_Property(googletest source_dir)
+  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
 
-# Get GTest source and binary directories from CMake project
+  # Specify MainTest's link libraries
+  ExternalProject_Get_Property(googletest binary_dir)
+  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
 
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+  # Create a libgtest target to be used as a dependency by test programs
+  add_library(libgtest IMPORTED STATIC GLOBAL)
+  add_dependencies(libgtest googletest)
 
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+  # Set libgtest properties
+  set_target_properties(libgtest PROPERTIES
+      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+  )
 
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+else()
 
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
-    "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-    "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+  message("CMake version equal or greater than 3.11.0")
 
+  include(FetchContent)
+
+  FetchContent_Declare(
+    googletest
+    GIT_REPOSITORY https://github.com/google/googletest.git 
+    GIT_TAG main)
+
+  SET(INSTALL_GTEST OFF) # Disable installation of googletest
+  FetchContent_MakeAvailable(googletest)
+    
+endif()
+  
 function(wolf_add_gtest target)
   add_executable(${target} ${ARGN})
-  add_dependencies(${target} libgtest)
-  target_link_libraries(${target} libgtest)
-
-  #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+  if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
+    add_dependencies(${target} libgtest)
+    target_link_libraries(${target} PUBLIC libgtest ${PLUGIN_NAME})
+    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+  else()
+    target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
+  endif()
   add_test(NAME ${target} COMMAND ${target})
 endfunction()