diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ed94b631561753a3a8eea35a562bf73c5728e82..e2fff5041837db4d13062888915930570eea625b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,20 +1,14 @@
-
 # 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)
 
 # The project name and the type of project
 PROJECT(laser_scan_utils)
 
 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_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(LIB_INSTALL_DIR lib/)
+# SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
 
 IF (NOT CMAKE_BUILD_TYPE)
   #SET(CMAKE_BUILD_TYPE "DEBUG")
@@ -28,9 +22,19 @@ SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
 
 #find dependencies
 FIND_PACKAGE(Eigen3 3.3 REQUIRED)
-FIND_PACKAGE(csm QUIET)
 FIND_PACKAGE(falkolib QUIET)
 
+find_package(PkgConfig)
+pkg_check_modules(csm QUIET csm )
+message(WARNING ${csm_FOUND})
+message(WARNING csm_LIBRARIES: ${csm_LIBRARIES})
+message(WARNING csm_LIBRARY: ${csm_LIBRARY})
+message(WARNING csm_LINK_LIBRARIES: ${csm_LINK_LIBRARIES})
+message(WARNING csm_LIBRARY_DIRS: ${csm_LIBRARY_DIRS})
+message(WARNING csm_INCLUDE_DIRS: ${csm_INCLUDE_DIRS})
+message(WARNING csm_INCLUDE_DIR: ${csm_INCLUDE_DIR})  # NOPE!
+
+
 #Build tests
 IF(NOT BUILD_TESTS)
   OPTION(BUILD_TESTS "Build Unit tests" ON)
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
deleted file mode 100644
index 953f9c5113d9139cefaaa508043f5d623072cb8d..0000000000000000000000000000000000000000
--- a/cmake_modules/Findcsm.cmake
+++ /dev/null
@@ -1,64 +0,0 @@
-FIND_PATH(
-    csm_INCLUDE_DIR
-    NAMES algos.h
-    PATHS /usr/local/include/csm)
-IF(csm_INCLUDE_DIR)
-  MESSAGE("Found csm include dirs: ${csm_INCLUDE_DIR}")
-ELSE(csm_INCLUDE_DIR)
-  MESSAGE("Couldn't find csm include dirs")
-ENDIF(csm_INCLUDE_DIR)
-
-FIND_LIBRARY(
-    csm_LIBRARY
-    NAMES libcsm.so libcsm.dylib
-    PATHS /usr/local/lib)
-IF(csm_LIBRARY)
-  MESSAGE("Found csm lib: ${csm_LIBRARY}")
-ELSE(csm_LIBRARY)
-  MESSAGE("Couldn't find csm lib")
-ENDIF(csm_LIBRARY)
-
-IF (csm_INCLUDE_DIR AND csm_LIBRARY)
-   SET(csm_FOUND TRUE)
- ELSE(csm_INCLUDE_DIR AND csm_LIBRARY)
-   set(csm_FOUND FALSE)
-ENDIF (csm_INCLUDE_DIR AND csm_LIBRARY)
-
-IF (csm_FOUND)
-   IF (NOT csm_FIND_QUIETLY)
-      MESSAGE(STATUS "Found csm: ${csm_LIBRARY}")
-   ENDIF (NOT csm_FIND_QUIETLY)
-ELSE (csm_FOUND)
-   IF (csm_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find csm")
-   ENDIF (csm_FIND_REQUIRED)
-ENDIF (csm_FOUND)
-
-
-macro(csm_report_not_found REASON_MSG)
-  set(csm_FOUND FALSE)
-  unset(csm_INCLUDE_DIR)
-  unset(csm_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 (csm_FIND_QUIETLY)
-    message(STATUS "Ignoring csm dependency - " ${REASON_MSG} ${ARGN})
-  elseif(csm_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find csm - " ${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 csm - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(csm_report_not_found)
-
-if(NOT csm_FOUND)
-  csm_report_not_found("Something went wrong while setting up csm.")
-endif(NOT csm_FOUND)
-# Set the include directories for csm (itself).
-set(csm_FOUND TRUE)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3aa65ec369a4c8ec2031fd8d539624b038a4fb64..61267cf59257798ed7b64620a16fd31a664ade37 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,12 +1,12 @@
 
 MESSAGE("Starting laser_scan_utils CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
 #include directories
-INCLUDE_DIRECTORIES(. /usr/local/include)
-INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(.)
+
 IF(csm_FOUND)
-    INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
+    INCLUDE_DIRECTORIES(${csm_INCLUDE_DIRS})
 ENDIF(csm_FOUND)
 IF(faramotics_FOUND)
     INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
@@ -90,6 +90,7 @@ SET(SRCS
   
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
 IF(csm_FOUND)
   target_link_libraries(${PROJECT_NAME} ${csm_LIBRARY})
 ENDIF(csm_FOUND)
@@ -116,5 +117,5 @@ INSTALL(FILES ${HDRS_BASE}
 INSTALL(FILES ${HDRS}
       DESTINATION include/iri-algorithms/laser_scan_utils)
 
-#install Find*.cmake
-INSTALL(FILES ../Findlaser_scan_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+# #install Find*.cmake
+# INSTALL(FILES ../Findlaser_scan_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 0f66e7183ad47f8fe9401f04d9941f6f12a1b3f3..bd9211fad7399d593b2651c025d30e22e7ee8f1b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,15 +1,13 @@
 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
 #Include directories
 INCLUDE_DIRECTORIES(../src)
 INCLUDE_DIRECTORIES(/data)
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
 FIND_PACKAGE(PythonLibs QUIET)
 INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${csm_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${falkolib_INCLUDE_DIRS})
 
 ############# USE THIS TEST AS AN EXAMPLE ####################
 #                                                            #
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 1bfcc9c1a097aa0b8d5d685c2e292e1680b544e5..fcd2a6744904ab1dcbe4b16cd00be06646fa787e 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,80 @@
-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 OFF)
 
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+  # 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()
 
-# 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()
+  # 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
+  )
 
-# 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
-)
+  # Get GTest source and binary directories from CMake project
 
-# Get GTest source and binary directories from CMake project
+  # Specify include dir
+  ExternalProject_Get_Property(googletest source_dir)
+  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
 
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+  # Specify MainTest's link libraries
+  ExternalProject_Get_Property(googletest binary_dir)
+  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
 
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest 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)
 
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+  # Set libgtest properties
+  set_target_properties(libgtest PROPERTIES
+      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+  )
 
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
-    "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-    "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+else()
 
+  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)
+
+  FetchContent_MakeAvailable(googletest)
+    
+endif()
+  
 function(laser_scan_utils_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} libgtest ${PLUGIN_NAME})
+    target_include_directories(${target} ${GTEST_INCLUDE_DIRS})
+  else()
+    target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
+  endif()
   add_test(NAME ${target} COMMAND ${target})
 endfunction()