diff --git a/CMakeLists.txt b/CMakeLists.txt
index 985d8706065cfb582fd0151e0cdfd1cadb7cda9a..9e43e79a6e604fb296d59b37db77aba9b4b8c1f7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,7 +8,6 @@ SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 set(INCLUDE_INSTALL_DIR include/iri-algorithms)
 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")
@@ -21,11 +20,14 @@ SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
 SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
 
 #find dependencies
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
 FIND_PACKAGE(falkolib QUIET)
 find_package(PkgConfig)
 pkg_check_modules(csm QUIET csm )
 link_directories(${csm_LIBRARY_DIRS})
+# keep only the files in a lib folder: by default the "." is in csm_LIBRARY_DIRS, 
+# which is not acceptable for target_link_directories 
+list(FILTER csm_LIBRARY_DIRS INCLUDE REGEX "/lib")
 
 
 #Build tests
@@ -57,20 +59,6 @@ endif()
 INCLUDE_DIRECTORIES("include")
 INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
 
-IF(csm_FOUND)
-  MESSAGE(STATUS "CSM lib found")
-  INCLUDE_DIRECTORIES(${csm_INCLUDE_DIRS})
-ENDIF(csm_FOUND)
-IF(faramotics_FOUND)
-  MESSAGE(STATUS "FARAMOTICS lib found")
-  INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-IF(falkolib_FOUND)
-  MESSAGE(STATUS "FALKO lib found:")
-  add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
-  INCLUDE_DIRECTORIES(${falkolib_INCLUDE_DIRS})
-  LINK_DIRECTORIES(${falkolib_LIBRARY_DIRS})
-ENDIF(falkolib_FOUND)
 
 #headers
 SET(HDRS_BASE
@@ -94,7 +82,6 @@ SET(HDRS
     include/laser_scan_utils/scan_segment.h
     include/laser_scan_utils/loop_closure_base.h
     include/laser_scan_utils/scene_base.h
-    
     )
   IF(csm_FOUND)
     SET(HDRS ${HDRS}
@@ -143,12 +130,18 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
 
 #Link the created libraries
 #=============================================================
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} Eigen3::Eigen)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC Eigen3::Eigen)
+
 IF(csm_FOUND)
-  TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${csm_LIBRARIES})
+  MESSAGE(STATUS "CSM lib found")
+  TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC ${csm_LIBRARIES})
+  target_include_directories(${PROJECT_NAME} PUBLIC ${csm_INCLUDE_DIRS})
+  target_link_directories(${PROJECT_NAME} PUBLIC ${csm_LIBRARY_DIRS})
 ENDIF(csm_FOUND)
 IF(falkolib_FOUND)
-  TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${falkolib_LIBRARY})
+  MESSAGE(STATUS "FALKO lib found:")
+  TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC ${falkolib_LIBRARY})
+  target_include_directories(${PROJECT_NAME} PUBLIC ${falkolib_INCLUDE_DIRS})
 ENDIF(falkolib_FOUND)
 
 
@@ -209,7 +202,7 @@ IF(BUILD_TESTS)
   	add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
-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/Findlaser_scan_utils.cmake b/Findlaser_scan_utils.cmake
deleted file mode 100644
index 7404338873ac262162217c06ebe8a2463fed0901..0000000000000000000000000000000000000000
--- a/Findlaser_scan_utils.cmake
+++ /dev/null
@@ -1,28 +0,0 @@
-FIND_PATH(
-    laser_scan_utils_INCLUDE_DIRS
-    NAMES laser_scan_utils.h
-    PATHS /usr/local/include/iri-algorithms/laser_scan_utils)
-#change INCLUDE_DIRS to its parent directory
-get_filename_component(laser_scan_utils_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS} DIRECTORY)
-
-FIND_LIBRARY(
-    laser_scan_utils_LIBRARY
-    NAMES laser_scan_utils
-    PATHS /usr/lib /usr/local/lib) 
-
-IF (laser_scan_utils_INCLUDE_DIRS AND laser_scan_utils_LIBRARY)
-   SET(laser_scan_utils_FOUND TRUE)
-   SET(laser_scan_utils_INCLUDE_DIR ${laser_scan_utils_INCLUDE_DIRS})
-   SET(laser_scan_utils_LIBRARIES ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_INCLUDE_DIRS AND laser_scan_utils_LIBRARY)
-
-IF (laser_scan_utils_FOUND)
-   IF (NOT laser_scan_utils_FIND_QUIETLY)
-      MESSAGE(STATUS "Found laser_scan_utils: ${laser_scan_utils_LIBRARY}")
-   ENDIF (NOT laser_scan_utils_FIND_QUIETLY)
-ELSE (laser_scan_utils_FOUND)
-   IF (laser_scan_utils_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find laser_scan_utils")
-   ENDIF (laser_scan_utils_FIND_REQUIRED)
-ENDIF (laser_scan_utils_FOUND)
-
diff --git a/cmake_modules/laser_scan_utilsConfig.cmake.in b/cmake_modules/laser_scan_utilsConfig.cmake.in
index 621f1e5b68ffb9bb42a615926b59134c9b64962c..699253a8747f367e40732cfbab17ded58218e1b5 100644
--- a/cmake_modules/laser_scan_utilsConfig.cmake.in
+++ b/cmake_modules/laser_scan_utilsConfig.cmake.in
@@ -3,8 +3,6 @@ set(@PROJECT_NAME@_VERSION 0.0.1)
 
 @PACKAGE_INIT@
 
-FIND_DEPENDENCY(falkolib REQUIRED)
-# FIND_DEPENDENCY(csm REQUIRED)  # does not work!
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake")
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index bd9211fad7399d593b2651c025d30e22e7ee8f1b..7358d24f757af241d6da1dd9d7c92197040c8257 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -4,6 +4,7 @@ add_subdirectory(gtest)
 #Include directories
 INCLUDE_DIRECTORIES(../src)
 INCLUDE_DIRECTORIES(/data)
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
 FIND_PACKAGE(PythonLibs QUIET)
 INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${csm_INCLUDE_DIRS})
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index fcd2a6744904ab1dcbe4b16cd00be06646fa787e..cbcb594321e7aafc1dcb5854842aec19328d9d76 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -72,7 +72,7 @@ function(laser_scan_utils_add_gtest target)
   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})
+    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
   else()
     target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
   endif()