diff --git a/CMakeLists.txt b/CMakeLists.txt
index cd81990329ddee7e5b82b04ce5cab1a28b7a3b29..8f31b67b24874d51eaff83a20478104d9d15d904 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -135,7 +135,7 @@ ENDIF(cereal_FOUND)
 
 
 # YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
+FIND_PACKAGE(YamlCpp REQUIRED)
 IF(YAMLCPP_FOUND)
     MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
 ELSEIF(YAMLCPP_FOUND)
@@ -194,7 +194,9 @@ ELSE (SPDLOG_INCLUDE_DIR)
 ENDIF (SPDLOG_INCLUDE_DIR)
 
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-include_directories("include")
+INCLUDE_DIRECTORIES("include")
+INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
+
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
@@ -212,10 +214,6 @@ IF(Suitesparse_FOUND)
     INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
 ENDIF(Suitesparse_FOUND)
 
-IF(YAMLCPP_FOUND)
-    INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
 IF(GLOG_FOUND)
 INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
 ENDIF(GLOG_FOUND)
@@ -351,6 +349,7 @@ SET(HDRS_DTASSC
 
 SET(HDRS_YAML
   include/core/yaml/parser_yaml.hpp
+  include/core/yaml/yaml_conversion.h
   )
 #SOURCES
 SET(SRCS_PROBLEM
@@ -439,6 +438,8 @@ SET(SRCS_SOLVER
   src/solver/solver_manager.cpp
   )
 SET(SRCS_YAML
+  src/yaml/processor_odom_3D_yaml.cpp
+  src/yaml/sensor_odom_3D_yaml.cpp
   )
 #OPTIONALS
 #optional HDRS and SRCS
@@ -474,18 +475,6 @@ IF (Suitesparse_FOUND)
   #DOES NOTHING?!
   #ADD_SUBDIRECTORY(solver_suitesparse)
 ENDIF(Suitesparse_FOUND)
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
-  # headers
-  SET(HDRS_YAML ${HDRS_YAML}
-    include/core/yaml/yaml_conversion.h
-    )
-  # sources
-  SET(SRCS_YAML ${SRCS_YAML}
-    src/yaml/processor_odom_3D_yaml.cpp
-    src/yaml/sensor_odom_3D_yaml.cpp
-    )
-ENDIF(YAMLCPP_FOUND)
 
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME}