diff --git a/CMakeLists.txt b/CMakeLists.txt
index be6e8e18f785506fd29a7168092ecc0466e0086c..9addae60ce0d06a7bf154b7632bb26977b6875e6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,7 +55,6 @@ COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
 PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
 add_required_dependency("eigen3 >= 3.0.0")
-include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
 
 # Add a cache variable to allow not compiling and running tests
 set (RUN_TESTS TRUE CACHE BOOL "compile and run unit tests")
@@ -74,7 +73,6 @@ search_for_boost()
 # Optional dependencies
 add_optional_dependency("octomap >= 1.6")
 if (OCTOMAP_FOUND)
-  include_directories(SYSTEM ${OCTOMAP_INCLUDE_DIRS})
   link_directories(${OCTOMAP_LIBRARY_DIRS})
   SET(HPP_FCL_HAVE_OCTOMAP TRUE)
   add_definitions (-DHPP_FCL_HAVE_OCTOMAP)
@@ -92,14 +90,6 @@ else()
 endif()
 
 ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
-if(ASSIMP_FOUND)
-  if (NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
-    add_definitions(-DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
-    message(STATUS "Assimp version has unified headers")
-  else()
-    message(STATUS "Assimp version does not have unified headers")
-  endif()
-endif()
 
 SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/collision_data.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3ffd16884c128afe5a142b79b30a23376406f857..498332f12109bae7402d0a701a0ae25fc346f7c3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -82,8 +82,6 @@ set(${LIBRARY_NAME}_SOURCES
   mesh_loader/loader.cpp
   )
 
-# Declare boost include directories
-include_directories(SYSTEM ${Boost_INCLUDE_DIRS})
 link_directories(${Boost_LIBRARY_DIRS})
 
 add_library(${LIBRARY_NAME}
@@ -91,11 +89,22 @@ add_library(${LIBRARY_NAME}
   ${${LIBRARY_NAME}_SOURCES}
   )
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
+
+target_include_directories(${LIBRARY_NAME} SYSTEM PUBLIC
+  ${EIGEN3_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  $<$<BOOL:${HPP_FCL_HAVE_OCTOMAP}>:${OCTOMAP_INCLUDE_DIRS}>
+  )
+
 PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} assimp)
+if (NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
+  target_compile_definitions(${LIBRARY_NAME} PRIVATE -DHPP_FCL_USE_ASSIMP_UNIFIED_HEADER_NAMES)
+  message(STATUS "Assimp version has unified headers")
+else()
+  message(STATUS "Assimp version does not have unified headers")
+endif()
 IF(OCTOMAP_FOUND)
   PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} octomap)
 ENDIF(OCTOMAP_FOUND)
-#PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} flann)
-
 
 install(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2db77cc854e5afe7f8d759de2a459e4149a9338e..11618f7261a3547f77b7e228614dda392846547c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -20,9 +20,13 @@ macro(add_fcl_test test_name)
 endmacro(add_fcl_test)
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
-include_directories(SYSTEM ${Boost_INCLUDE_DIRS})
 
 add_library(utility STATIC utility.cpp)
+target_include_directories(utility SYSTEM PUBLIC
+  ${EIGEN3_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  $<$<BOOL:${HPP_FCL_HAVE_OCTOMAP}>:${OCTOMAP_INCLUDE_DIRS}>
+  )
 
 add_fcl_test(math math.cpp)