diff --git a/CMakeLists.txt b/CMakeLists.txt
index 920358e..b3c142e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,298 +1,28 @@
-cmake_minimum_required(VERSION 2.6)
+cmake_minimum_required(VERSION 2.8.12)
+project(libnabo)
 
-if (NOT CMAKE_VERSION VERSION_LESS "3.1")
-	cmake_policy(SET CMP0054 NEW)
-endif ()
-
-set(LIB_NAME nabo)
-project("lib${LIB_NAME}")
-
-# Extract version from header
-set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
-execute_process(
-	COMMAND grep "NABO_VERSION " nabo/nabo.h
-	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-	RESULT_VARIABLE GREP_VERSION_RESULT
-	OUTPUT_VARIABLE PROJECT_VERSION
-	OUTPUT_STRIP_TRAILING_WHITESPACE
-)
-if (NOT GREP_VERSION_RESULT EQUAL 0)
-	message(SEND_ERROR "Cannot grep version number: ${GREP_VERSION_RESULT}")
-endif ()
-string(REGEX REPLACE ".*\"(.*)\".*" "\\1" PROJECT_VERSION "${PROJECT_VERSION}" )
-
-if (NOT CMAKE_BUILD_TYPE)
-	message("-- No build type specified; defaulting to CMAKE_BUILD_TYPE=Release.")
-	set(CMAKE_BUILD_TYPE Release CACHE STRING
-	  "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
-	  FORCE)
-else ()
-	if (CMAKE_BUILD_TYPE STREQUAL "Debug")
-		message("\n=================================================================================")
-		message("\n-- Build type: Debug. Performance will be terrible!")
-		message("-- Add -DCMAKE_BUILD_TYPE=Release to the CMake command line to get an optimized build.")
-		message("\n=================================================================================")
-	endif ()
-endif ()
-
-if (NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
-	add_definitions(-O3)
-endif()
-
-# Documentation
-option(LIBNABO_BUILD_DOXYGEN "Build libnabo doxygen documentation" ON)
-if (LIBNABO_BUILD_DOXYGEN)
-  set(DOXYFILE_LATEX false)
-  include(UseDoxygen)
-endif()
-
-# Switch on warnings.
-if (CMAKE_CXX_COMPILER_ID STREQUAL "MSVC")
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Wall")
-else ()
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
-endif ()
-
-# Check the compiler version as we need C++11 support.
-if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
-	# using Clang
-	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.3")
-		message(FATAL_ERROR "Your clang compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version 3.3 or later is required")
-	endif ()
-elseif ()
-	# using AppleClang
-	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "5.1")
-		message(FATAL_ERROR "Your XCode environment has version ${CMAKE_CXX_COMPILER_VERSION}, while version 5.1 or later is required")
-	endif ()
-elseif ()
-	# using GCC
-	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "4.8.2")
-		message(FATAL_ERROR "Your g++ compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version 4.8.2 or later is required")
-	endif ()
-elseif ()
-	# using MSVC
-	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "19.0.23506")
-		message(FATAL_ERROR "Your Microsoft Visual C++ compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version MSVC 2015 Update 1+ is required")
-	endif ()
-endif ()
-
-# enable C++11 support.
-if (CMAKE_VERSION VERSION_LESS "3.1")
-	if (MSVC)
-		message(FATAL_ERROR "CMake version 3.1 or later is required to compile ${PROJECT_NAME} with Microsoft Visual C++")
-	endif ()
-	if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
-		set (CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
-	else ()
-		set (CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
-	endif ()
-else ()
-	set (CMAKE_CXX_STANDARD 11)
-endif ()
-
-include(GNUInstallDirs)
-
-# eigen 2 or 3
-find_path(EIGEN_INCLUDE_DIR Eigen/Core
-	/usr/local/include/eigen3
-	/usr/local/include/eigen2
-	/usr/local/include/eigen
-	/usr/include/eigen3
-	/usr/include/eigen2
-	/usr/include/eigen
-	/opt/local/include/eigen3
-)
-
-# optionally, opencl
-# OpenCL disabled as its code is not up-to-date with API
-set(USE_OPEN_CL FALSE CACHE BOOL "Set to TRUE to look for OpenCL")
-if (USE_OPEN_CL)
-	find_path(OPENCL_INCLUDE_DIR CL/cl.h
-		/usr/local/include
-		/usr/include
-	)
-	if (WIN32)
-		find_library(OPENCL_LIBRARIES opencl64)
-		if (!OPENCL_LIBRARIES)
-			find_library(OPENCL_LIBRARIES opencl32)
-		endif ()
-	else ()
-		find_library(OPENCL_LIBRARIES OpenCL ENV LD_LIBRARY_PATH)
-	endif ()
-	if (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
-		add_definitions(-DHAVE_OPENCL)
-		set(EXTRA_LIBS ${OPENCL_LIBRARIES} ${EXTRA_LIBS})
-		include_directories(${OPENCL_INCLUDE_DIR})
-		add_definitions(-DOPENCL_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}/nabo/opencl/\")
-		message("OpenCL enabled and found, enabling CL support")
-	else (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
-		message("OpenCL enabled but not found, disabling CL support")
-	endif ()
-else ()
-	message("OpenCL disabled, not looking for it")
-endif ()
-
-
-# main nabo lib
+# Source and header files.
 set(NABO_SRC
-	nabo/nabo.cpp
-	nabo/brute_force_cpu.cpp
-	nabo/kdtree_cpu.cpp
-	nabo/kdtree_opencl.cpp
+  nabo/nabo.cpp
+  nabo/brute_force_cpu.cpp
+  nabo/kdtree_cpu.cpp
+  nabo/kdtree_opencl.cpp
 )
-set(SHARED_LIBS FALSE CACHE BOOL "Set to TRUE to build shared library")
-if (SHARED_LIBS)
-	add_library(${LIB_NAME} SHARED ${NABO_SRC})
-	install(TARGETS ${LIB_NAME} LIBRARY DESTINATION lib)
-else ()
-	add_library(${LIB_NAME} STATIC ${NABO_SRC})
-	if (NOT MSVC)
-		target_compile_options(${LIB_NAME} PRIVATE -fPIC)
-	endif()
-	install(TARGETS ${LIB_NAME} ARCHIVE DESTINATION lib)
-endif ()
-set_target_properties(${LIB_NAME} PROPERTIES VERSION "${PROJECT_VERSION}" SOVERSION 1)
-
-target_include_directories(${LIB_NAME} PUBLIC
-	${EIGEN_INCLUDE_DIR}
-	$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
-	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
-  )
-
-# openmp
-set(USE_OPEN_MP TRUE CACHE BOOL "Set to FALSE to not use OpenMP")
-if (USE_OPEN_MP)
-	find_package(OpenMP)
-	if (OPENMP_FOUND)
-		target_compile_options(${LIB_NAME} PRIVATE -fopenmp ${OpenMP_CXX_FLAGS})
-		target_compile_definitions(${LIB_NAME} PRIVATE HAVE_OPENMP)
-		if (CMAKE_COMPILER_IS_GNUCC)
-			target_link_libraries(${LIB_NAME} PUBLIC gomp)
-		endif()
-	endif()
-endif ()
-
-
-# create doc before installing
-set(DOC_INSTALL_TARGET "share/doc/${PROJECT_NAME}/api" CACHE STRING "Target where to install doxygen documentation")
-install(FILES nabo/nabo.h DESTINATION include/nabo)
-install(FILES nabo/third_party/any.hpp DESTINATION include/nabo/third_party)
-install(FILES README.md DESTINATION share/doc/${PROJECT_NAME})
-if (DOXYGEN_FOUND)
-	install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc/html DESTINATION ${DOC_INSTALL_TARGET})
-endif (DOXYGEN_FOUND)
-
-enable_testing()
-
-
-option(LIBNABO_BUILD_EXAMPLES "Build libnabo examples" ON)
-if (LIBNABO_BUILD_EXAMPLES)
-  add_subdirectory(examples)
-endif()
-
-option(LIBNABO_BUILD_TESTS "Build libnabo tests" ON)
-if(LIBNABO_BUILD_TESTS)
-  add_subdirectory(tests)
-endif()
-
-option(LIBNABO_BUILD_PYTHON "Build libnabo python" ON)
-if(LIBNABO_BUILD_PYTHON)
-  add_subdirectory(python)
-endif()
-
-# Install catkin package.xml
-install(FILES package.xml DESTINATION share/libnabo)
-
-#=============================================
-# to allow find_package() on libnabo
-#=============================================
-#
-# the following case be used in an external project requiring libnabo:
-#  ...
-#  find_package(libnabo)
-#  include_directories(${libnabo_INCLUDE_DIRS})
-#  target_link_libraries(executableName ${libnabo_LIBRARIES})
-#  ...
-
-# NOTE: the following will support find_package for 1) local build (make) and 2) for installed files (make install)
-
-# 1- local build #
-
-# Register the local build in case one doesn't use "make install"
-export(PACKAGE libnabo)
-
-# 'make install' to the correct locations (provided by GNUInstallDirs).
-install(TARGETS ${LIB_NAME} EXPORT ${PROJECT_NAME}-targets
-    ARCHIVE  DESTINATION ${CMAKE_INSTALL_LIBDIR}
-    LIBRARY  DESTINATION ${CMAKE_INSTALL_LIBDIR}
-    RUNTIME  DESTINATION ${CMAKE_INSTALL_BINDIR})  # This is for Windows
-#install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
-
-add_library(${PROJECT_NAME}::${LIB_NAME} ALIAS ${LIB_NAME})
-
-# This makes the project importable from the install directory
-# Put config file in per-project dir (name MUST match), can also
-# just go into 'cmake'.
-install(
-	EXPORT ${PROJECT_NAME}-targets
-	DESTINATION share/${PROJECT_NAME}/cmake
-	NAMESPACE ${PROJECT_NAME}::
-)
-
-# This makes the project importable from the build directory
-export(
-	TARGETS ${LIB_NAME}
-	FILE ${PROJECT_NAME}-targets.cmake
-	NAMESPACE ${PROJECT_NAME}::
-)
-
-
-# Create variable with the library location
-set(libnabo_library $<TARGET_FILE:${LIB_NAME}>)
-
-# Create variable for the local build tree
-get_property(libnabo_include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
-# Configure & generate config file for local build tree
-configure_file(libnaboConfig.cmake.in
-	"${PROJECT_BINARY_DIR}/libnaboConfig.cmake.conf" @ONLY)
-file(GENERATE
-	OUTPUT "${PROJECT_BINARY_DIR}/libnaboConfig.cmake"
-	INPUT "${PROJECT_BINARY_DIR}/libnaboConfig.cmake.conf")
-
-# 2- installation build #
-
-# Change the library location for an install location
-set(libnabo_library ${CMAKE_INSTALL_PREFIX}/lib/$<TARGET_FILE_NAME:${LIB_NAME}>)
-
-# Change the include location for the case of an install location
-set(libnabo_include_dirs ${CMAKE_INSTALL_PREFIX}/include)
-
-# We put the generated file for installation in a different repository (i.e., ./CMakeFiles/)
-configure_file(libnaboConfig.cmake.in
-	"${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake.conf" @ONLY)
-file(GENERATE
-	OUTPUT "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake"
-	INPUT "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake.conf")
-
-# The same versioning file can be used for both cases
-configure_file(libnaboConfigVersion.cmake.in
-	"${PROJECT_BINARY_DIR}/libnaboConfigVersion.cmake" @ONLY)
-
-install(FILES
-	"${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake"
-	"${PROJECT_BINARY_DIR}/libnaboConfigVersion.cmake"
-	DESTINATION share/libnabo/cmake COMPONENT dev)
-
-
-#=============================================
-# Add uninstall target
-#=============================================
-if (NOT TARGET uninstall)
-  configure_file(
-	  "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in"
-	  "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
-	  IMMEDIATE @ONLY)
 
-  add_custom_target(uninstall
-	  COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
+file(GLOB_RECURSE NABO_HEADERS "nabo/*.h")
+
+# Global compiler flags.
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+# Build type options:
+#	* catkin.
+#	* cmake.
+#	* ament (future).
+set(BUILD_TYPE "cmake")
+if (BUILD_TYPE STREQUAL "catkin")
+  include(cmake/CatkinBuild.cmake)
+else()
+  # For a pure-cmake build, follow the instructions on the package.xml file, which
+  # specify how to set the buildtool to cmake and export the package accordingly.
+  include(cmake/PureCMakeBuild.cmake)
 endif()
diff --git a/cmake/CatkinBuild.cmake b/cmake/CatkinBuild.cmake
new file mode 100644
index 0000000..6ae02e2
--- /dev/null
+++ b/cmake/CatkinBuild.cmake
@@ -0,0 +1,78 @@
+# Set compiler flags
+set(CMAKE_CXX_STANDARD 14)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+if (NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+  add_definitions(-O3)
+endif(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+
+# Find catkin macros and libraries
+find_package(catkin REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS python)
+
+find_package(OpenMP)
+if (NOT OpenMP_FOUND)
+  message("OpenMP was not found. It is highly recommended to build libnabo with OpenMP support.")
+endif()
+
+# Catkin package macro
+catkin_package(
+  INCLUDE_DIRS
+    ${CMAKE_SOURCE_DIR}
+    ${EIGEN3_INCLUDE_DIR}
+  LIBRARIES
+    nabo
+)
+
+########################
+## Library definition ##
+########################
+# Nabo
+add_library(nabo
+  ${NABO_SRC}
+  ${NABO_HEADERS}
+)
+target_include_directories(nabo PUBLIC
+  ${CMAKE_SOURCE_DIR}
+  ${CMAKE_SOURCE_DIR}/nabo
+)
+target_include_directories(nabo SYSTEM PRIVATE
+  ${EIGEN3_INCLUDE_DIR}
+  ${Boost_INCLUDE_DIRS}
+  ${OpenMP_CXX_INCLUDE_DIRS}
+)
+target_link_libraries(nabo PRIVATE
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${OpenMP_CXX_LIBRARIES}
+)
+target_compile_options(nabo PRIVATE
+  "${OpenMP_CXX_FLAGS}"
+)
+target_compile_definitions(nabo PRIVATE
+  -DHAVE_OPENMP=${OpenMP_FOUND}
+)
+
+# Python bindings
+add_subdirectory(python)
+
+#############
+## Install ##
+#############
+install(
+  TARGETS
+    nabo
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY
+    ${CMAKE_SOURCE_DIR}/nabo/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/nabo
+  FILES_MATCHING
+    PATTERN "*.h"
+    PATTERN "*.hpp"
+)
\ No newline at end of file
diff --git a/cmake/PureCMakeBuild.cmake b/cmake/PureCMakeBuild.cmake
new file mode 100644
index 0000000..89b8a0b
--- /dev/null
+++ b/cmake/PureCMakeBuild.cmake
@@ -0,0 +1,290 @@
+cmake_minimum_required(VERSION 2.6)
+
+if (NOT CMAKE_VERSION VERSION_LESS "3.1")
+	cmake_policy(SET CMP0054 NEW)
+endif ()
+
+set(LIB_NAME nabo)
+project("lib${LIB_NAME}")
+
+# Extract version from header
+set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+execute_process(
+	COMMAND grep "NABO_VERSION " nabo/nabo.h
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	RESULT_VARIABLE GREP_VERSION_RESULT
+	OUTPUT_VARIABLE PROJECT_VERSION
+	OUTPUT_STRIP_TRAILING_WHITESPACE
+)
+if (NOT GREP_VERSION_RESULT EQUAL 0)
+	message(SEND_ERROR "Cannot grep version number: ${GREP_VERSION_RESULT}")
+endif ()
+string(REGEX REPLACE ".*\"(.*)\".*" "\\1" PROJECT_VERSION "${PROJECT_VERSION}" )
+
+if (NOT CMAKE_BUILD_TYPE)
+	message("-- No build type specified; defaulting to CMAKE_BUILD_TYPE=Release.")
+	set(CMAKE_BUILD_TYPE Release CACHE STRING
+	  "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
+	  FORCE)
+else ()
+	if (CMAKE_BUILD_TYPE STREQUAL "Debug")
+		message("\n=================================================================================")
+		message("\n-- Build type: Debug. Performance will be terrible!")
+		message("-- Add -DCMAKE_BUILD_TYPE=Release to the CMake command line to get an optimized build.")
+		message("\n=================================================================================")
+	endif ()
+endif ()
+
+if (NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+	add_definitions(-O3)
+endif()
+
+# Documentation
+option(LIBNABO_BUILD_DOXYGEN "Build libnabo doxygen documentation" ON)
+if (LIBNABO_BUILD_DOXYGEN)
+  set(DOXYFILE_LATEX false)
+  include(cmake/UseDoxygen)
+endif()
+
+# Switch on warnings.
+if (CMAKE_CXX_COMPILER_ID STREQUAL "MSVC")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Wall")
+else ()
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
+endif ()
+
+# Check the compiler version as we need C++11 support.
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+	# using Clang
+	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "3.3")
+		message(FATAL_ERROR "Your clang compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version 3.3 or later is required")
+	endif ()
+elseif ()
+	# using AppleClang
+	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "5.1")
+		message(FATAL_ERROR "Your XCode environment has version ${CMAKE_CXX_COMPILER_VERSION}, while version 5.1 or later is required")
+	endif ()
+elseif ()
+	# using GCC
+	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "4.8.2")
+		message(FATAL_ERROR "Your g++ compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version 4.8.2 or later is required")
+	endif ()
+elseif ()
+	# using MSVC
+	if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS "19.0.23506")
+		message(FATAL_ERROR "Your Microsoft Visual C++ compiler has version ${CMAKE_CXX_COMPILER_VERSION}, while version MSVC 2015 Update 1+ is required")
+	endif ()
+endif ()
+
+# enable C++11 support.
+if (CMAKE_VERSION VERSION_LESS "3.1")
+	if (MSVC)
+		message(FATAL_ERROR "CMake version 3.1 or later is required to compile ${PROJECT_NAME} with Microsoft Visual C++")
+	endif ()
+	if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+		set (CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
+	else ()
+		set (CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+	endif ()
+else ()
+	set (CMAKE_CXX_STANDARD 11)
+endif ()
+
+include(GNUInstallDirs)
+
+# eigen 2 or 3
+find_path(EIGEN_INCLUDE_DIR Eigen/Core
+	/usr/local/include/eigen3
+	/usr/local/include/eigen2
+	/usr/local/include/eigen
+	/usr/include/eigen3
+	/usr/include/eigen2
+	/usr/include/eigen
+	/opt/local/include/eigen3
+)
+
+# optionally, opencl
+# OpenCL disabled as its code is not up-to-date with API
+set(USE_OPEN_CL FALSE CACHE BOOL "Set to TRUE to look for OpenCL")
+if (USE_OPEN_CL)
+	find_path(OPENCL_INCLUDE_DIR CL/cl.h
+		/usr/local/include
+		/usr/include
+	)
+	if (WIN32)
+		find_library(OPENCL_LIBRARIES opencl64)
+		if (!OPENCL_LIBRARIES)
+			find_library(OPENCL_LIBRARIES opencl32)
+		endif ()
+	else ()
+		find_library(OPENCL_LIBRARIES OpenCL ENV LD_LIBRARY_PATH)
+	endif ()
+	if (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
+		add_definitions(-DHAVE_OPENCL)
+		set(EXTRA_LIBS ${OPENCL_LIBRARIES} ${EXTRA_LIBS})
+		include_directories(${OPENCL_INCLUDE_DIR})
+		add_definitions(-DOPENCL_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}/nabo/opencl/\")
+		message("OpenCL enabled and found, enabling CL support")
+	else (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
+		message("OpenCL enabled but not found, disabling CL support")
+	endif ()
+else ()
+	message("OpenCL disabled, not looking for it")
+endif ()
+
+set(SHARED_LIBS FALSE CACHE BOOL "Set to TRUE to build shared library")
+if (SHARED_LIBS)
+	add_library(${LIB_NAME} SHARED ${NABO_SRC})
+	install(TARGETS ${LIB_NAME} LIBRARY DESTINATION lib)
+else ()
+	add_library(${LIB_NAME} STATIC ${NABO_SRC})
+	if (NOT MSVC)
+		target_compile_options(${LIB_NAME} PRIVATE -fPIC)
+	endif()
+	install(TARGETS ${LIB_NAME} ARCHIVE DESTINATION lib)
+endif ()
+set_target_properties(${LIB_NAME} PROPERTIES VERSION "${PROJECT_VERSION}" SOVERSION 1)
+
+target_include_directories(${LIB_NAME} PUBLIC
+	${EIGEN_INCLUDE_DIR}
+	$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
+  )
+
+# openmp
+set(USE_OPEN_MP TRUE CACHE BOOL "Set to FALSE to not use OpenMP")
+if (USE_OPEN_MP)
+	find_package(OpenMP)
+	if (OPENMP_FOUND)
+		target_compile_options(${LIB_NAME} PRIVATE -fopenmp ${OpenMP_CXX_FLAGS})
+		target_compile_definitions(${LIB_NAME} PRIVATE HAVE_OPENMP)
+		if (CMAKE_COMPILER_IS_GNUCC)
+			target_link_libraries(${LIB_NAME} PUBLIC gomp)
+		endif()
+	endif()
+endif ()
+
+
+# create doc before installing
+set(DOC_INSTALL_TARGET "share/doc/${PROJECT_NAME}/api" CACHE STRING "Target where to install doxygen documentation")
+install(FILES nabo/nabo.h DESTINATION include/nabo)
+install(FILES nabo/third_party/any.hpp DESTINATION include/nabo/third_party)
+install(FILES README.md DESTINATION share/doc/${PROJECT_NAME})
+if (DOXYGEN_FOUND)
+	install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc/html DESTINATION ${DOC_INSTALL_TARGET})
+endif (DOXYGEN_FOUND)
+
+enable_testing()
+
+
+option(LIBNABO_BUILD_EXAMPLES "Build libnabo examples" ON)
+if (LIBNABO_BUILD_EXAMPLES)
+  add_subdirectory(examples)
+endif()
+
+option(LIBNABO_BUILD_TESTS "Build libnabo tests" ON)
+if(LIBNABO_BUILD_TESTS)
+  add_subdirectory(tests)
+endif()
+
+option(LIBNABO_BUILD_PYTHON "Build libnabo python" ON)
+if(LIBNABO_BUILD_PYTHON)
+  add_subdirectory(python)
+endif()
+
+# Install catkin package.xml
+install(FILES package.xml DESTINATION share/libnabo)
+
+#=============================================
+# to allow find_package() on libnabo
+#=============================================
+#
+# the following case be used in an external project requiring libnabo:
+#  ...
+#  find_package(libnabo)
+#  include_directories(${libnabo_INCLUDE_DIRS})
+#  target_link_libraries(executableName ${libnabo_LIBRARIES})
+#  ...
+
+# NOTE: the following will support find_package for 1) local build (make) and 2) for installed files (make install)
+
+# 1- local build #
+
+# Register the local build in case one doesn't use "make install"
+export(PACKAGE libnabo)
+
+# 'make install' to the correct locations (provided by GNUInstallDirs).
+install(TARGETS ${LIB_NAME} EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE  DESTINATION ${CMAKE_INSTALL_LIBDIR}
+    LIBRARY  DESTINATION ${CMAKE_INSTALL_LIBDIR}
+    RUNTIME  DESTINATION ${CMAKE_INSTALL_BINDIR})  # This is for Windows
+#install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+add_library(${PROJECT_NAME}::${LIB_NAME} ALIAS ${LIB_NAME})
+
+# This makes the project importable from the install directory
+# Put config file in per-project dir (name MUST match), can also
+# just go into 'cmake'.
+install(
+	EXPORT ${PROJECT_NAME}-targets
+	DESTINATION share/${PROJECT_NAME}/cmake
+	NAMESPACE ${PROJECT_NAME}::
+)
+
+# This makes the project importable from the build directory
+export(
+	TARGETS ${LIB_NAME}
+	FILE ${PROJECT_NAME}-targets.cmake
+	NAMESPACE ${PROJECT_NAME}::
+)
+
+
+# Create variable with the library location
+set(libnabo_library $<TARGET_FILE:${LIB_NAME}>)
+
+# Create variable for the local build tree
+get_property(libnabo_include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
+# Configure & generate config file for local build tree
+configure_file(cmake/libnaboConfig.cmake.in
+	"${PROJECT_BINARY_DIR}/libnaboConfig.cmake.conf" @ONLY)
+file(GENERATE
+	OUTPUT "${PROJECT_BINARY_DIR}/libnaboConfig.cmake"
+	INPUT "${PROJECT_BINARY_DIR}/libnaboConfig.cmake.conf")
+
+# 2- installation build #
+
+# Change the library location for an install location
+set(libnabo_library ${CMAKE_INSTALL_PREFIX}/lib/$<TARGET_FILE_NAME:${LIB_NAME}>)
+
+# Change the include location for the case of an install location
+set(libnabo_include_dirs ${CMAKE_INSTALL_PREFIX}/include)
+
+# We put the generated file for installation in a different repository (i.e., ./CMakeFiles/)
+configure_file(cmake/libnaboConfig.cmake.in
+	"${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake.conf" @ONLY)
+file(GENERATE
+	OUTPUT "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake"
+	INPUT "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake.conf")
+
+# The same versioning file can be used for both cases
+configure_file(cmake/libnaboConfigVersion.cmake.in
+	"${PROJECT_BINARY_DIR}/libnaboConfigVersion.cmake" @ONLY)
+
+install(FILES
+	"${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/libnaboConfig.cmake"
+	"${PROJECT_BINARY_DIR}/libnaboConfigVersion.cmake"
+	DESTINATION share/libnabo/cmake COMPONENT dev)
+
+
+#=============================================
+# Add uninstall target
+#=============================================
+if (NOT TARGET uninstall)
+  configure_file(
+	  "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
+	  "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake"
+	  IMMEDIATE @ONLY)
+
+  add_custom_target(uninstall
+	  COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
+endif()
diff --git a/UseDoxygen.cmake b/cmake/UseDoxygen.cmake
similarity index 100%
rename from UseDoxygen.cmake
rename to cmake/UseDoxygen.cmake
diff --git a/cmake_uninstall.cmake.in b/cmake/cmake_uninstall.cmake.in
similarity index 100%
rename from cmake_uninstall.cmake.in
rename to cmake/cmake_uninstall.cmake.in
diff --git a/libnaboConfig.cmake.in b/cmake/libnaboConfig.cmake.in
similarity index 100%
rename from libnaboConfig.cmake.in
rename to cmake/libnaboConfig.cmake.in
diff --git a/libnaboConfigVersion.cmake.in b/cmake/libnaboConfigVersion.cmake.in
similarity index 100%
rename from libnaboConfigVersion.cmake.in
rename to cmake/libnaboConfigVersion.cmake.in
diff --git a/package.xml b/package.xml
index 202939d..36c4b7d 100644
--- a/package.xml
+++ b/package.xml
@@ -1,5 +1,4 @@
-<?xml version="1.0"?>
-<package>
+<package format="3">
 	<name>libnabo</name>
 	<version>1.0.7</version>
 	<description>
@@ -11,16 +10,18 @@
 	<url type="repository">https://github.com/ethz-asl/libnabo</url>
 	<author email="stephane.magnenat@mavt.ethz.ch">Stéphane Magnenat</author>
 
-	<!-- The *_depend tags are used to specify dependencies -->
-	<buildtool_depend>cmake</buildtool_depend>
-	<build_depend>eigen</build_depend>
+	<!-- If a pure-cmake build is desired, the buildtool should be switched to 'cmake' -->
+	<buildtool_depend>catkin</buildtool_depend>
+	
 	<build_depend>boost</build_depend>
-	<run_depend>eigen</run_depend>
-	<run_depend>boost</run_depend>
-	<run_depend>catkin</run_depend>
+	<build_depend>eigen</build_depend>
+	<build_depend>libomp-dev</build_depend>
 
+	<!-- For a pure-cmake build the package must be exported explicitly -->
+	<!-- Thus the following block of code must be un-commented -->
+	<!--
 	<export>
-		<!-- Specify that this is not really a ROS package -->
 		<build_type>cmake</build_type>
 	</export>
+	-->
 </package>