diff --git a/CMakeLists.txt b/CMakeLists.txt
index 614e52d2d623b656801b4a6371eafaa36cbb4db3..7baa17a2573071d910505834d0a567a417b63aaa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -97,18 +97,19 @@ FIND_PACKAGE(Threads REQUIRED)
 
 FIND_PACKAGE(wolf REQUIRED)
 
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
-    MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-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)
     MESSAGE("yaml-cpp Library NOT FOUND!")
 ENDIF(YAMLCPP_FOUND)
 
+FIND_PACKAGE(Ceres QUIET) #Ceres is not required
+IF(Ceres_FOUND)
+    MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
+ENDIF(Ceres_FOUND)
+
+
 FIND_PATH(
     Suitesparse_INCLUDE_DIRS
     NAMES SuiteSparse_config.h
@@ -144,8 +145,9 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
 # ============EXAMPLE==================
 INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIRS})
-include_directories(BEFORE "include")
+INCLUDE_DIRECTORIES(BEFORE "include")
 INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)