diff --git a/.cproject b/.cproject
index de7640655e2ef3d7bc156252969e68d1f5251715..222a3595bd88ad061b0ec6539d7fa9a95b638ba1 100644
--- a/.cproject
+++ b/.cproject
@@ -22,8 +22,8 @@
 							<tool id="cdt.managedbuild.tool.gnu.archiver.base.599567389" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
 							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.1572359948" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base">
 								<option id="gnu.cpp.compiler.option.include.paths.1770487778" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/vision_utils/src}&quot;"/>
 									<listOptionValue builtIn="false" value="/usr/local/include"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/Vision Utils/src}&quot;"/>
 								</option>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.466329004" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
 							</tool>
@@ -36,7 +36,7 @@
 							</tool>
 							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1546364764" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
 							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.674742229" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base">
-								<option id="gnu.cpp.link.option.paths.1264112011" superClass="gnu.cpp.link.option.paths" valueType="libPaths">
+								<option id="gnu.cpp.link.option.paths.1264112011" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" valueType="libPaths">
 									<listOptionValue builtIn="false" value="/usr/local/lib"/>
 								</option>
 								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.224259993" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
@@ -70,18 +70,18 @@
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
 	<storageModule moduleId="scannerConfiguration">
 		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1736073220;cdt.managedbuild.toolchain.gnu.base.1736073220.1390770886;cdt.managedbuild.tool.gnu.c.compiler.base.1459107576;cdt.managedbuild.tool.gnu.c.compiler.input.1695301683">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1819918851;cdt.managedbuild.toolchain.gnu.base.1819918851.1318867162;cdt.managedbuild.tool.gnu.cpp.compiler.base.1649772065;cdt.managedbuild.tool.gnu.cpp.compiler.input.146329834">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.207681558;cdt.managedbuild.toolchain.gnu.base.207681558.1208625085;cdt.managedbuild.tool.gnu.c.compiler.base.1439246049;cdt.managedbuild.tool.gnu.c.compiler.input.637694474">
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1736073220;cdt.managedbuild.toolchain.gnu.base.1736073220.1390770886;cdt.managedbuild.tool.gnu.c.compiler.base.1459107576;cdt.managedbuild.tool.gnu.c.compiler.input.1695301683">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.727026548;cdt.managedbuild.toolchain.gnu.base.727026548.1071101131;cdt.managedbuild.tool.gnu.c.compiler.base.61708613;cdt.managedbuild.tool.gnu.c.compiler.input.166078074">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.207681558;cdt.managedbuild.toolchain.gnu.base.207681558.1208625085;cdt.managedbuild.tool.gnu.c.compiler.base.1439246049;cdt.managedbuild.tool.gnu.c.compiler.input.637694474">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1244102678;cdt.managedbuild.toolchain.gnu.base.1244102678.1450977940;cdt.managedbuild.tool.gnu.cpp.compiler.base.1572359948;cdt.managedbuild.tool.gnu.cpp.compiler.input.466329004">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
@@ -91,13 +91,13 @@
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.727026548;cdt.managedbuild.toolchain.gnu.base.727026548.1071101131;cdt.managedbuild.tool.gnu.cpp.compiler.base.433122623;cdt.managedbuild.tool.gnu.cpp.compiler.input.341288586">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1244102678;cdt.managedbuild.toolchain.gnu.base.1244102678.1450977940;cdt.managedbuild.tool.gnu.c.compiler.base.1838202852;cdt.managedbuild.tool.gnu.c.compiler.input.964584931">
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1736073220;cdt.managedbuild.toolchain.gnu.base.1736073220.1390770886;cdt.managedbuild.tool.gnu.cpp.compiler.base.1526891046;cdt.managedbuild.tool.gnu.cpp.compiler.input.1116382079">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1819918851;cdt.managedbuild.toolchain.gnu.base.1819918851.1318867162;cdt.managedbuild.tool.gnu.c.compiler.base.593702953;cdt.managedbuild.tool.gnu.c.compiler.input.392834553">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1736073220;cdt.managedbuild.toolchain.gnu.base.1736073220.1390770886;cdt.managedbuild.tool.gnu.cpp.compiler.base.1526891046;cdt.managedbuild.tool.gnu.cpp.compiler.input.1116382079">
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.base.1244102678;cdt.managedbuild.toolchain.gnu.base.1244102678.1450977940;cdt.managedbuild.tool.gnu.c.compiler.base.1838202852;cdt.managedbuild.tool.gnu.c.compiler.input.964584931">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 	</storageModule>
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 93fe002bdb1a70e0e6f6af40a2063d0e1896afd3..59f82f0751a469d54b4fd45f0684cfa1e1583f6b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,10 @@
 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+## OPTIONS ##
+OPTION(BUILD_TESTS "Build Unit tests" ON)
+OPTION(BUILD_EXAMPLES "Build examples" ON)
+OPTION(PRINT_INFO_VU "Print vision utils information" OFF)
 
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW) 
@@ -15,10 +20,14 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
 
 IF (NOT CMAKE_BUILD_TYPE)
- SET(CMAKE_BUILD_TYPE "RELEASE") 
+ SET(CMAKE_BUILD_TYPE "DEBUG") 
 ENDIF (NOT CMAKE_BUILD_TYPE)
 MESSAGE(STATUS "Compilation type: ${CMAKE_BUILD_TYPE}")
  
+#Set Flags
+SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT") 
+ 
 if(UNIX)
   # GCC is not strict enough by default, so enable most of the warnings.
   set(CMAKE_CXX_FLAGS
@@ -39,6 +48,16 @@ else()
         message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 
+## Tests ##
+if(BUILD_TESTS)
+    MESSAGE("Building tests.")
+    # Enables testing for this directory and below.
+    # Note that ctest expects to find a test file in the build directory root.
+    # Therefore, this command should be in the source directory root.
+    #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
+    enable_testing()
+endif()
+
 ADD_SUBDIRECTORY(src)
 
 FIND_PACKAGE(Doxygen)
@@ -76,4 +95,19 @@ ELSE(UNIX)
   )
 ENDIF(UNIX)
 
+IF (UNIX)
+  SET(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
+  SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}-dev")
+  SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
+  SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
+  SET(CPACK_GENERATOR "DEB")
+  SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "asantamaria@iri.upc.edu")
+  SET(CPACK_SET_DESTDIR "ON")  # Necessary because of the absolute install paths
+  INCLUDE(CPack)
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "packaging only implemented in unix"
+    TARGET  uninstall)
+ENDIF(UNIX)
+
 
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ceae6cc75cfcf9a2dedda8395159caef3510fb2b..01673715166b2ef9670f03bb6ab1ece336d21924 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,5 +1,19 @@
-# Specific definitions
-#ADD_DEFINITIONS(-DPRINT_INFO_VU)
+#Start Vision Utils build
+MESSAGE("Starting Vision Utils CMakeLists ...")
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+
+IF(BUILD_EXAMPLES OR BUILD_TESTS)
+  SET(_VU_ROOT_DIR ${CMAKE_SOURCE_DIR})
+ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
+
+IF(PRINT_INFO_VU)
+  ADD_DEFINITIONS(-DPRINT_INFO_VU)
+ENDIF(PRINT_INFO_VU)  
+
+IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
+  SET(_VU_DEBUG true)
+  ADD_DEFINITIONS(-D_VU_DEBUG)
+ENDIF()
 
 # library source files
 SET(sources
@@ -277,7 +291,14 @@ INSTALL(FILES ${headers_alg_activesearch} DESTINATION include/${PROJECT_NAME}/al
 INSTALL(FILES ../cmake_modules/Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 INSTALL(FILES "${VU_CONFIG_DIR}/config.h" DESTINATION include/${PROJECT_NAME}/_internal)
 
-# examples of usage
-ADD_SUBDIRECTORY(examples)
-
+## Tests ##
+if(BUILD_TESTS)
+    MESSAGE("Building tests.")
+    ADD_SUBDIRECTORY(test)
+endif()
 
+## Examples ##
+IF(BUILD_EXAMPLES)
+  MESSAGE("Building examples.")
+  ADD_SUBDIRECTORY(examples)
+ENDIF(BUILD_EXAMPLES)
diff --git a/src/algorithms/activesearch/alg_activesearch.cpp b/src/algorithms/activesearch/alg_activesearch.cpp
index 1305f091492ee7f876e7677a4928ce2e06b4fa5f..b55b0b1fd99ff0318c6deca886076eaa32cae3e1 100644
--- a/src/algorithms/activesearch/alg_activesearch.cpp
+++ b/src/algorithms/activesearch/alg_activesearch.cpp
@@ -163,13 +163,13 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa
 	       	{
 	       		// Keep best in cell
 	       		KeyPointVector list_keypoints = kps;
-	       		unsigned int index = 0;
+
 	       		// cv::KeyPointsFilter keypoint_filter;
 	       		// keypoint_filter.retainBest(kps,1);
 	       		retainBest(kps,1);
-	       		for(unsigned int ii = 0; ii < list_keypoints.size(); ii++)
-	       			if(list_keypoints[ii].pt == kps[0].pt)
-	       				index = ii;
+
+	       		// Check if point exist in list
+	       		int index = existsIn(kps[0], list_keypoints, 1.0);
 
 	       		if(kps[0].response > params_ptr_->min_response_new_feature)
 	            {
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5f9db7574f403e2ca8aa69eef75fb44379d9bfd3
--- /dev/null
+++ b/src/test/CMakeLists.txt
@@ -0,0 +1,27 @@
+# Retrieve googletest from github & compile
+add_subdirectory(gtest)
+
+# Include gtest directory.
+include_directories(${GTEST_INCLUDE_DIRS})
+
+############# USE THIS TEST AS AN EXAMPLE #################
+#                                                         #
+# Create a specific test executable for gtest_example     #
+vu_add_gtest(gtest_example gtest_example.cpp)           #
+target_link_libraries(gtest_example ${PROJECT_NAME})      #
+#                                                         #
+###########################################################
+
+
+################# ADD YOUR TESTS BELOW ####################
+#                                                         #
+#           ==== IN ALPHABETICAL ORDER! ====              #
+#                                                         #
+
+# ------- First Core classes ----------
+
+# ------- Now Derived classes ----------
+
+# ROI test
+  vu_add_gtest(gtest_roi_ORB gtest_roi_ORB.cpp)
+  target_link_libraries(gtest_roi_ORB ${PROJECT_NAME})
\ No newline at end of file
diff --git a/src/test/Test_ORB.png b/src/test/Test_ORB.png
new file mode 100644
index 0000000000000000000000000000000000000000..016141f5309c1ed34a61d71cfa63b130ea90aa8f
Binary files /dev/null and b/src/test/Test_ORB.png differ
diff --git a/src/test/data/Test_ORB.png b/src/test/data/Test_ORB.png
new file mode 100644
index 0000000000000000000000000000000000000000..016141f5309c1ed34a61d71cfa63b130ea90aa8f
Binary files /dev/null and b/src/test/data/Test_ORB.png differ
diff --git a/src/test/data/roi_orb.yaml b/src/test/data/roi_orb.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..96331a697399081f17c2ad818b9c50a55d66a25b
--- /dev/null
+++ b/src/test/data/roi_orb.yaml
@@ -0,0 +1,10 @@
+detector:
+  type: "ORB"         
+  nfeatures: 20
+  scale factor: 1.2
+  nlevels: 8
+  edge threshold: 16   # 16
+  first level: 0 
+  WTA_K: 2            # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
+  score type: 0       #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
+  patch size: 16     # 31
diff --git a/src/test/gtest/CMakeLists.txt b/src/test/gtest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1e7b400f688727ed4d5e7bdec839e9897cd9f3d9
--- /dev/null
+++ b/src/test/gtest/CMakeLists.txt
@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 2.8.8)
+project(gtest_builder C CXX)
+
+# We need thread support
+#find_package(Threads REQUIRED)
+
+# Enable ExternalProject CMake module
+include(ExternalProject)
+
+set(GTEST_FORCE_SHARED_CRT ON)
+set(GTEST_DISABLE_PTHREADS OFF)
+
+# For some reason I need to disable PTHREADS
+# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
+# This is a known issue for MinGW :
+# https://github.com/google/shaderc/pull/174
+#if(MINGW)
+    set(GTEST_DISABLE_PTHREADS ON)
+#endif()
+
+# Download GoogleTest
+ExternalProject_Add(googletest
+    GIT_REPOSITORY https://github.com/google/googletest.git
+    # TIMEOUT 1 # We'll try this
+    CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+    -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+    -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+    -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+    -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+    -DBUILD_GTEST=ON
+    PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+    # Disable install step
+    INSTALL_COMMAND ""
+    # UPDATE_DISCONNECTED 1
+)
+
+# Get GTest source and binary directories from CMake project
+
+# Specify include dir
+ExternalProject_Get_Property(googletest source_dir)
+set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+
+# Specify MainTest's link libraries
+ExternalProject_Get_Property(googletest binary_dir)
+set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+
+# Create a libgtest target to be used as a dependency by test programs
+add_library(libgtest IMPORTED STATIC GLOBAL)
+add_dependencies(libgtest googletest)
+
+# Set libgtest properties
+set_target_properties(libgtest PROPERTIES
+    "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+    "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+)
+
+function(vu_add_gtest target)
+  add_executable(${target} ${ARGN})
+  add_dependencies(${target} libgtest)
+  target_link_libraries(${target} libgtest)
+
+  #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+  add_test(NAME ${target} COMMAND ${target})
+endfunction()
diff --git a/src/test/gtest_example.cpp b/src/test/gtest_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c4ae7a14daf9d9923ec040dea90629eb5c32b5d
--- /dev/null
+++ b/src/test/gtest_example.cpp
@@ -0,0 +1,20 @@
+#include "utils_gtest.h"
+
+TEST(TestTest, DummyTestExample)
+{
+  EXPECT_FALSE(false);
+
+  ASSERT_TRUE(true);
+
+  int my_int = 5;
+
+  ASSERT_EQ(my_int, 5);
+
+  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/test/gtest_roi_ORB.cpp b/src/test/gtest_roi_ORB.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4c9bcd01c2f99e6ebb98b4289f852ea134e7d4bf
--- /dev/null
+++ b/src/test/gtest_roi_ORB.cpp
@@ -0,0 +1,141 @@
+/**
+ * \file gtest_roi_ORB.cpp
+ *
+ *  Created on: Nov 28, 2016
+ *      \author: jsola
+ */
+
+#include "utils_gtest.h"
+
+// vision utils includes
+#include "../vision_utils.h"
+#include "../detectors.h"
+
+// std include
+#include <vector>
+#include <map>
+
+TEST(RoiORB, LoadImageFromFile)
+{
+    cv::Mat image;
+    std::string filename, vu_root;
+    vu_root = _VU_ROOT_DIR;
+    filename = vu_root + "/src/test/data/Test_ORB.png";
+    image = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+
+    ASSERT_TRUE(image.data)<< "Failed to load image " << filename << std::endl;
+}
+
+// Test_ORB.png image file has the following interest points that should be detected:
+// 0  [ 65.6 , 100.6 ]
+// 1  [ 164 , 100.6 ]
+// 2  [ 266 , 100.6 ]
+// 3  [ 365.2 , 100.6 ]
+// 4  [ 467.04 , 101.32 ]
+// 5  [ 565.6 , 100.6 ]
+// 6  [ 71.6 , 237.8 ]
+// 7  [ 164.8 , 237.8 ]
+// 8  [ 251.36 , 239.24 ]
+// 9  [ 330.2 , 237.8 ]
+// 10 [ 349 , 270 ]     // point #10 is out of the ROI scanned area and should not be detected
+// 11 [ 584 , 237.8 ]
+// 12 [ 467.2 , 455.4 ]
+// 13 [ 566 , 455.4 ]
+PointVector points_to_check({
+    cv::Point2f(  65.6 , 100.6 ),
+    cv::Point2f( 164   , 100.6 ),
+    cv::Point2f( 266   , 100.6 ),
+    cv::Point2f( 365.2 , 100.6 ),
+    cv::Point2f( 467   , 101.3 ),
+    cv::Point2f( 565.6 , 100.6 ),
+    cv::Point2f(  71.6 , 237.8 ),
+    cv::Point2f( 164.8 , 237.8 ),
+    cv::Point2f( 250   , 239.2 ),
+    cv::Point2f( 330.2 , 237.8 ),
+    cv::Point2f( 349   , 270   ),
+    cv::Point2f( 584   , 237.8 ),
+    cv::Point2f( 467.2 , 455.4 ),
+    cv::Point2f( 566   , 455.4 )
+});
+
+TEST(RoiORB, RoiBounds)
+{
+    cv::Mat image;
+    std::string filename, vu_root;
+    vu_root = _VU_ROOT_DIR;
+    filename = vu_root + "/src/test/data/Test_ORB.png";
+    image = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+
+    ASSERT_TRUE(image.data)<< "failed to load image " << filename << std::endl;
+
+    unsigned int img_width = image.cols;
+
+    // Define detector
+    std::string yaml_file_params = vu_root + "/src/test/data/roi_orb.yaml";
+    std::string det_name = vision_utils::readYamlType(yaml_file_params, "detector");
+    vision_utils::DetectorBasePtr det_ptr = vision_utils::setupDetector(det_name, det_name + " detector", yaml_file_params);
+    det_ptr = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr);
+
+    std::vector<cv::KeyPoint> target_keypoints;
+    cv::KeyPointsFilter keypoint_filter;
+
+    int roi_x;
+    int roi_y;
+    int roi_width = 50;
+    int roi_heigth = 50;
+
+    Eigen::VectorXi roi_center_y(3); roi_center_y << 102 , 250 , 476;
+
+    std::map<int, cv::Point2f> points_found;
+
+    for (int ii = 0; ii<roi_center_y.size() ; ii++)
+    {
+        roi_y = roi_center_y(ii) - roi_width/2;
+
+        for(roi_x = 0; roi_x < img_width; roi_x += 5)
+        {
+            cv::Rect roi(roi_x, roi_y, roi_width, roi_heigth);
+            cv::Rect roi_inflated = roi;
+
+            // Detect features in ROI
+            target_keypoints = det_ptr->detect(image, roi_inflated);
+
+            // Keep only one KP in ROI
+            if (!target_keypoints.empty())
+            {
+                keypoint_filter.retainBest(target_keypoints,1);
+                cv::Point2f pnt = target_keypoints[0].pt;
+
+                int j = vision_utils::existsIn(pnt, points_to_check, 2.0);
+
+                ASSERT_GE(j, 0);
+
+                // append the keypoint to the list of keypoints found
+                if (j >= 0)
+                    points_found[j] = pnt;
+            }
+
+#ifdef _VU_DEBUG
+            cv::Mat image_graphics = image.clone();
+            cv::drawKeypoints(image_graphics,target_keypoints,image_graphics);
+            cv::rectangle(image_graphics, roi, cv::Scalar(255.0, 0.0, 255.0), 1, 8, 0);
+            cv::rectangle(image_graphics, roi_inflated, cv::Scalar(255.0, 255.0, 0.0), 1, 8, 0);
+            cv::imshow("test",image_graphics);
+            cv::waitKey(1);
+#endif
+            }
+    }
+
+    // check that at least all keypoints in the list except #10 have been detected
+    // (note: #10 is out of the ROI)
+    std::vector<int> nn({0,1,2,3,4,5,6,7,8,9,11,12,13});
+    for (int n : nn)
+        ASSERT_TRUE(points_found.count(n));
+    ASSERT_FALSE(points_found.count(10));
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/test/gtest_vision_utils.cpp b/src/test/gtest_vision_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3b8cf3e07e552d185432b83883250b463ff0e4da
--- /dev/null
+++ b/src/test/gtest_vision_utils.cpp
@@ -0,0 +1,34 @@
+#include "utils_gtest.h"
+#include "../vision_utils.h"
+
+TEST(VisionUtils, DummyTestExample)
+{
+  EXPECT_FALSE(false);
+
+  ASSERT_TRUE(true);
+
+  int my_int = 5;
+
+  ASSERT_EQ(my_int, 5);
+
+  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+TEST(VisionUtils, DummyTestExample)
+{
+  EXPECT_FALSE(false);
+
+  ASSERT_TRUE(true);
+
+  int my_int = 5;
+
+  ASSERT_EQ(my_int, 5);
+
+  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/test/utils_gtest.h b/src/test/utils_gtest.h
new file mode 100644
index 0000000000000000000000000000000000000000..7716bf0d7386c3883368a527b248a54beaf62847
--- /dev/null
+++ b/src/test/utils_gtest.h
@@ -0,0 +1,132 @@
+#ifndef VU_UTILS_GTEST_H
+#define VU_UTILS_GTEST_H
+
+#include <gtest/gtest.h>
+
+// Macros for testing equalities and inequalities.
+//
+//    * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
+//    * {ASSERT|EXPECT}_NE(v1, v2):           Tests that v1 != v2
+//    * {ASSERT|EXPECT}_LT(v1, v2):           Tests that v1 < v2
+//    * {ASSERT|EXPECT}_LE(v1, v2):           Tests that v1 <= v2
+//    * {ASSERT|EXPECT}_GT(v1, v2):           Tests that v1 > v2
+//    * {ASSERT|EXPECT}_GE(v1, v2):           Tests that v1 >= v2
+//
+// C String Comparisons.  All tests treat NULL and any non-NULL string
+// as different.  Two NULLs are equal.
+//
+//    * {ASSERT|EXPECT}_STREQ(s1, s2):     Tests that s1 == s2
+//    * {ASSERT|EXPECT}_STRNE(s1, s2):     Tests that s1 != s2
+//    * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
+//    * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
+//
+// Macros for comparing floating-point numbers.
+//
+//    * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
+//         Tests that two float values are almost equal.
+//    * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
+//         Tests that two double values are almost equal.
+//    * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
+//         Tests that v1 and v2 are within the given distance to each other.
+//
+// These predicate format functions work on floating-point values, and
+// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
+//
+//   EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
+//
+// Macros that execute statement and check that it doesn't generate new fatal
+// failures in the current thread.
+//
+//    * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
+
+
+// http://stackoverflow.com/a/29155677
+
+namespace testing
+{
+namespace internal
+{
+enum GTestColor
+{
+  COLOR_DEFAULT,
+  COLOR_RED,
+  COLOR_GREEN,
+  COLOR_YELLOW
+};
+
+extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
+
+#define PRINTF(...) \
+  do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
+  "[          ] "); \
+  testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
+  while(0)
+
+// C++ stream interface
+class TestCout : public std::stringstream
+{
+public:
+  ~TestCout()
+  {
+    PRINTF("%s\n", str().c_str());
+  }
+};
+
+/* Usage :
+
+TEST(Test, Foo)
+{
+  // the following works but prints default stream
+  EXPECT_TRUE(false) << "Testing Stream.";
+
+  // or you can play with AINSI color code
+  EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream.";
+
+  // or use the above defined macros
+
+  PRINTF("Hello world");
+
+  // or
+
+  TEST_COUT << "Hello world";
+}
+
+*/
+#define TEST_COUT testing::internal::TestCout()
+
+} // namespace internal
+
+/* Macros related to testing Eigen classes:
+ */
+#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+                  return (lhs - rhs).isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+                  return (lhs - rhs).isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+
+#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+                   MatrixXs er = lhs - rhs; \
+                   er(2) = pi2pi((Scalar)er(2)); \
+                   return er.isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+                   MatrixXs er = lhs - rhs; \
+                   er(2) = pi2pi((Scalar)er(2)); \
+                   return er.isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+} // namespace testing
+
+
+#endif /* VU_UTILS_GTEST_H */
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
index 2b68a93e9c091f8aad16977dc94bf702e2522433..6cb1fd3d07f481f8e90015ffd7db23bbeea7eddf 100644
--- a/src/vision_utils.cpp
+++ b/src/vision_utils.cpp
@@ -151,6 +151,32 @@ void whoHasMoved(const PointVector& _pt_vec_1, const PointVector& _pt_vec_2, Poi
     }
 }
 
+int existsIn(const cv::Point2f& p, const PointVector p_vec, const double& pixel_tol)
+{
+	double pixel_tol_squared = pixel_tol*pixel_tol;
+    for (int ii = 0; ii < p_vec.size(); ii++)
+    {
+    	double dx = p.x - p_vec[ii].x;
+    	double dy = p.y - p_vec[ii].y;
+        if ( dx*dx + dy*dy < pixel_tol_squared) // match based on Euclidean distance
+            return ii;
+    }
+    return -1; // -1 marks 'not found'
+}
+
+int existsIn(const cv::KeyPoint& p, const KeyPointVector p_vec, const double& pixel_tol)
+{
+	double pixel_tol_squared = pixel_tol*pixel_tol;
+    for (int ii = 0; ii < p_vec.size(); ii++)
+    {
+    	double dx = p.pt.x - p_vec[ii].pt.x;
+    	double dy = p.pt.y - p_vec[ii].pt.y;
+        if ( dx*dx + dy*dy < pixel_tol_squared) // match based on Euclidean distance
+            return ii;
+    }
+    return -1; // -1 marks 'not found'
+}
+
 void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius, const cv::Scalar& _color, const int& _thickness)
 {
     for (auto kp : _kp_vec)
diff --git a/src/vision_utils.h b/src/vision_utils.h
index 9f10fc48c4a54a1326eb7a1da833d1a04af208d2..d9e883dc9b66667a6097753df04e38cb0e5c87dd 100644
--- a/src/vision_utils.h
+++ b/src/vision_utils.h
@@ -24,17 +24,20 @@
 #include <eigen3/Eigen/Sparse>
 
 // OpenCV
-#include <opencv2/core/core.hpp>
-#include <opencv2/core/types.hpp>
+#include <opencv2/opencv.hpp>
 #include <opencv2/core/eigen.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/xfeatures2d/nonfree.hpp>
+#include <opencv2/line_descriptor.hpp>
 #include <opencv2/xfeatures2d.hpp>
-#include <opencv2/line_descriptor/descriptor.hpp>
-#include <opencv2/calib3d.hpp>
+
+// REMOVE
+//#include <opencv2/core.hpp>
+//#include <opencv2/core/types.hpp>
+//#include <opencv2/imgproc/imgproc.hpp>
+//#include <opencv2/highgui/highgui.hpp>
+//#include <opencv2/features2d/features2d.hpp>
+//#include <opencv2/features2d.hpp>
+//#include <opencv2/xfeatures2d/nonfree.hpp>
+//#include <opencv2/calib3d.hpp>
 
 typedef std::vector<cv::Point2f> PointVector;
 typedef std::vector<cv::KeyPoint> KeyPointVector;
@@ -153,6 +156,9 @@ PointVector KPToP(const KeyPointVector& _kp_vec);
 void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2);
 void whoHasMoved(const PointVector& _pts1, const PointVector& _pts2, PointVector& _common_pts, PointVector& _new_in_pts2);
 
+int existsIn(const cv::Point2f& p, const PointVector p_vec, const double& pixel_tol);
+int existsIn(const cv::KeyPoint& p, const KeyPointVector p_vec, const double& pixel_tol);
+
 void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius=5, const cv::Scalar& _color = cv::Scalar(0, 0, 255), const int& _thickness=-1);
 
 void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255) );