diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e63ddd8b5e0c43622cec18e9a0a0aa61e00f9103
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+/Default/
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ec4ad5c483d1a7139415559eb58d113fbdd5bae7
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,61 @@
+# Pre-requisites about cmake itself
+CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+
+if(COMMAND cmake_policy)
+  cmake_policy(SET CMP0005 NEW) 
+  cmake_policy(SET CMP0003 NEW)
+   
+endif(COMMAND cmake_policy)
+
+# The project name and the type of project
+PROJECT(vision_utils)
+
+SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
+SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+SET(CMAKE_INSTALL_PREFIX /usr/local)
+
+IF (NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE "DEBUG") 
+ENDIF (NOT CMAKE_BUILD_TYPE)
+
+SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
+
+ADD_SUBDIRECTORY(src)
+
+FIND_PACKAGE(Doxygen)
+
+ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
+
+ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
+
+IF (UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "distribution clean"
+    COMMAND make ARGS clean
+    COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
+    TARGET  distclean
+  )
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "distclean only implemented in unix"
+    TARGET  distclean
+  )
+ENDIF(UNIX)
+
+ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
+
+IF (UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "uninstall package"
+    COMMAND xargs ARGS rm < install_manifest.txt
+    TARGET  uninstall
+  )
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "uninstall only implemented in unix"
+    TARGET  uninstall
+  )
+ENDIF(UNIX)
+
+
diff --git a/Findvision_utils.cmake b/Findvision_utils.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..b6e75119644b076ff69ca3943d90689be325577c
--- /dev/null
+++ b/Findvision_utils.cmake
@@ -0,0 +1,21 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(vision_utils_INCLUDE_DIR vision_utils.h /usr/include/vision_utils /usr/local/include/vision_utils)
+
+FIND_LIBRARY(vision_utils_LIBRARY
+    NAMES vision_utils
+    PATHS /usr/lib /usr/local/lib /usr/local/lib/vision_utils) 
+
+IF (vision_utils_INCLUDE_DIR AND vision_utils_LIBRARY)
+   SET(vision_utils_FOUND TRUE)
+ENDIF (vision_utils_INCLUDE_DIR AND vision_utils_LIBRARY)
+
+IF (vision_utils_FOUND)
+   IF (NOT vision_utils_FIND_QUIETLY)
+      MESSAGE(STATUS "Found vision_utils: ${vision_utils_LIBRARY}")
+   ENDIF (NOT vision_utils_FIND_QUIETLY)
+ELSE (vision_utils_FOUND)
+   IF (vision_utils_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find vision_utils")
+   ENDIF (vision_utils_FIND_REQUIRED)
+ENDIF (vision_utils_FOUND)
+
diff --git a/ReadMe.md b/ReadMe.md
new file mode 100644
index 0000000000000000000000000000000000000000..00d38de2b7519046c17ec060acab862127e5b69a
--- /dev/null
+++ b/ReadMe.md
@@ -0,0 +1,112 @@
+# vision_utils 
+
+
+## Introduction 
+
+#### Pre-Requisites
+
+This package requires of the following libraries and packages  
+
+  * [cmake](http://www.cmake.org), a cross-platform build system
+  * [doxygen](http://www.doxygen.org) and 
+    [graphviz](http://www.graphviz.org) to generate the documentation
+  * stdc++  
+
+Under linux all of these utilities are available in ready-to-use packages.  
+
+Under MacOS most of the packages are available via [fink](http://www.finkproject.org).  
+
+#### Compilation
+
+  Just download this package, uncompress it, and execute  
+
+    `cd build`  
+    `cmake ..`  
+
+to generate the makefile and then  
+
+    `make`  
+
+to obtain the shared library and also all the example programs.  
+
+The *cmake* only needs to be executed once (make will automatically call *cmake* 
+if you modify one of the `CMakeList.txt` files).  
+
+To generate this documentation type  
+
+    `make doc`  
+
+The files in the `build` directory are genetated by *cmake* and *make* 
+and can be safely removed.  
+After doing so you will need to call cmake manually again.  
+
+#### Configuration  
+
+The default build mode is DEBUG. That is, objects and executables include debug information.  
+
+The RELEASE build mode optimizes for speed. To build in this mode execute  
+
+    `cmake .. -DCMAKE_BUILD_TYPE=RELEASE`  
+
+The release mode will be kept until next time cmake is executed.  
+
+#### Installation
+
+In order to be able to use the library, it it necessary to copy it into the system. 
+To do that, execute  
+
+    `make install`  
+
+as root and the shared libraries will be copied to `/usr/local/lib/<vision_utils>` directory
+and the header files will be copied to `/usr/local/include/<vision_utils>` directory. At 
+this point, the library may be used by any user.  
+
+To remove the library from the system, exceute  
+
+    `make uninstall`  
+
+as root, and all the associated files will be removed from the system.  
+
+## Customization  
+
+To build a new application using these library, first it is necessary to locate if the library
+has been installed or not using the following command in your `CMakeLists.txt`  
+
+    `FIND_PACKAGE(vision_utils REQUIRED)`  
+
+In the case that the package is present, it is necessary to add the header files directory to
+the include directory path by using  
+
+    `INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})`
+
+Finally, it is also nevessary to link with the desired libraries by using the following command  
+
+    `TARGET_LINK_LIBRARIES(<executable name> ${vision_utils_LIBRARY})`
+
+## License
+
+This package is licensed under a [GPL 3.0 License](http://www.gnu.org/licenses/gpl.html)
+
+#### Disclaimer
+
+Copyright (C) 2017  
+[http://www.angelsantamaria.eu](http://www.angelsantamaria.eu)  
+All rights reserved.  
+
+This file is part of vision_utils library  
+
+vision_utils library is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+at your option) any later version.  
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU Lesser General Public License for more details.  
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program.  If not, see [gnu licenses](http://www.gnu.org/licenses/)  
+
+
+
diff --git a/bin/.gitignore b/bin/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..94548af5beba7825284af746324c8dc5b2f1ea31
--- /dev/null
+++ b/bin/.gitignore
@@ -0,0 +1,3 @@
+*
+*/
+!.gitignore
diff --git a/build/.gitignore b/build/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..94548af5beba7825284af746324c8dc5b2f1ea31
--- /dev/null
+++ b/build/.gitignore
@@ -0,0 +1,3 @@
+*
+*/
+!.gitignore
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
new file mode 100755
index 0000000000000000000000000000000000000000..347af2ef86bf4786d49b13d48c12146b0d001b01
--- /dev/null
+++ b/doc/doxygen.conf
@@ -0,0 +1,251 @@
+# Doxyfile 1.5.5
+
+@INCLUDE_PATH = ../doc/
+@INCLUDE = doxygen_project_name.conf
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING      = UTF-8
+PROJECT_NUMBER         = 
+OUTPUT_DIRECTORY       = ../doc
+CREATE_SUBDIRS         = NO
+OUTPUT_LANGUAGE        = English
+BRIEF_MEMBER_DESC      = YES
+REPEAT_BRIEF           = NO
+ABBREVIATE_BRIEF       = 
+ALWAYS_DETAILED_SEC    = NO
+INLINE_INHERITED_MEMB  = NO
+FULL_PATH_NAMES        = YES
+STRIP_FROM_PATH        = 
+STRIP_FROM_INC_PATH    = 
+SHORT_NAMES            = NO
+JAVADOC_AUTOBRIEF      = NO
+QT_AUTOBRIEF           = NO
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP         = NO
+INHERIT_DOCS           = YES
+SEPARATE_MEMBER_PAGES  = NO
+TAB_SIZE               = 8
+ALIASES                = 
+OPTIMIZE_OUTPUT_FOR_C  = YES
+OPTIMIZE_OUTPUT_JAVA   = NO
+OPTIMIZE_FOR_FORTRAN   = NO
+OPTIMIZE_OUTPUT_VHDL   = NO
+BUILTIN_STL_SUPPORT    = NO
+CPP_CLI_SUPPORT        = NO
+SIP_SUPPORT            = NO
+DISTRIBUTE_GROUP_DOC   = NO
+SUBGROUPING            = YES
+TYPEDEF_HIDES_STRUCT   = NO
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL            = NO
+EXTRACT_PRIVATE        = NO
+EXTRACT_STATIC         = NO
+EXTRACT_LOCAL_CLASSES  = YES
+EXTRACT_LOCAL_METHODS  = NO
+EXTRACT_ANON_NSPACES   = NO
+HIDE_UNDOC_MEMBERS     = NO
+HIDE_UNDOC_CLASSES     = NO
+HIDE_FRIEND_COMPOUNDS  = NO
+HIDE_IN_BODY_DOCS      = NO
+INTERNAL_DOCS          = NO
+CASE_SENSE_NAMES       = YES
+HIDE_SCOPE_NAMES       = NO
+SHOW_INCLUDE_FILES     = YES
+INLINE_INFO            = YES
+SORT_MEMBER_DOCS       = NO
+SORT_BRIEF_DOCS        = NO
+SORT_GROUP_NAMES       = NO
+SORT_BY_SCOPE_NAME     = NO
+GENERATE_TODOLIST      = YES
+GENERATE_TESTLIST      = YES
+GENERATE_BUGLIST       = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS       = 
+MAX_INITIALIZER_LINES  = 30
+SHOW_USED_FILES        = YES
+SHOW_DIRECTORIES       = YES
+FILE_VERSION_FILTER    = 
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET                  = YES
+WARNINGS               = YES
+WARN_IF_UNDOCUMENTED   = YES
+WARN_IF_DOC_ERROR      = YES
+WARN_NO_PARAMDOC       = NO
+WARN_FORMAT            = "$file:$line: $text"
+WARN_LOGFILE           = 
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT                  = ../src \
+                         ../doc/main.dox
+INPUT_ENCODING         = UTF-8
+FILE_PATTERNS          = *.c \
+                         *.h \
+                         *.cpp
+RECURSIVE              = YES
+EXCLUDE                = 
+EXCLUDE_SYMLINKS       = NO
+EXCLUDE_PATTERNS       = *.tab.c \
+                         *.tab.h \
+                         lex* \
+                         *glr.h \
+                         *llr.h \
+                         *glr.c \
+                         *llr.c \
+                         *general.h
+EXCLUDE_SYMBOLS        = 
+EXAMPLE_PATH           = ../src/examples
+EXAMPLE_PATTERNS       = 
+EXAMPLE_RECURSIVE      = NO
+IMAGE_PATH             = ../doc/images
+INPUT_FILTER           = 
+FILTER_PATTERNS        = 
+FILTER_SOURCE_FILES    = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER         = YES
+INLINE_SOURCES         = NO
+STRIP_CODE_COMMENTS    = YES
+REFERENCED_BY_RELATION = YES
+REFERENCES_RELATION    = YES
+REFERENCES_LINK_SOURCE = YES
+USE_HTAGS              = NO
+VERBATIM_HEADERS       = YES
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX     = YES
+COLS_IN_ALPHA_INDEX    = 5
+IGNORE_PREFIX          = 
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML          = YES
+HTML_OUTPUT            = html
+HTML_FILE_EXTENSION    = .html
+HTML_HEADER            = 
+HTML_FOOTER            = 
+HTML_STYLESHEET        = 
+HTML_ALIGN_MEMBERS     = YES
+GENERATE_HTMLHELP      = NO
+GENERATE_DOCSET        = NO
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+HTML_DYNAMIC_SECTIONS  = NO
+CHM_FILE               = 
+HHC_LOCATION           = 
+GENERATE_CHI           = NO
+BINARY_TOC             = NO
+TOC_EXPAND             = NO
+DISABLE_INDEX          = NO
+ENUM_VALUES_PER_LINE   = 4
+GENERATE_TREEVIEW      = NO
+TREEVIEW_WIDTH         = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX         = NO
+LATEX_OUTPUT           = latex
+LATEX_CMD_NAME         = latex
+MAKEINDEX_CMD_NAME     = makeindex
+COMPACT_LATEX          = NO
+PAPER_TYPE             = a4
+EXTRA_PACKAGES         = 
+LATEX_HEADER           = 
+PDF_HYPERLINKS         = YES
+USE_PDFLATEX           = NO
+LATEX_BATCHMODE        = NO
+LATEX_HIDE_INDICES     = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF           = NO
+RTF_OUTPUT             = rtf
+COMPACT_RTF            = NO
+RTF_HYPERLINKS         = NO
+RTF_STYLESHEET_FILE    = 
+RTF_EXTENSIONS_FILE    = 
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN           = NO
+MAN_OUTPUT             = man
+MAN_EXTENSION          = .3
+MAN_LINKS              = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML           = NO
+XML_OUTPUT             = xml
+XML_SCHEMA             = 
+XML_DTD                = 
+XML_PROGRAMLISTING     = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF   = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD       = NO
+PERLMOD_LATEX          = NO
+PERLMOD_PRETTY         = YES
+PERLMOD_MAKEVAR_PREFIX = 
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor   
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING   = YES
+MACRO_EXPANSION        = NO
+EXPAND_ONLY_PREDEF     = NO
+SEARCH_INCLUDES        = YES
+INCLUDE_PATH           = 
+INCLUDE_FILE_PATTERNS  = 
+PREDEFINED             = _USE_MPI=1
+EXPAND_AS_DEFINED      = 
+SKIP_FUNCTION_MACROS   = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references   
+#---------------------------------------------------------------------------
+TAGFILES               = 
+GENERATE_TAGFILE       = 
+ALLEXTERNALS           = NO
+EXTERNAL_GROUPS        = YES
+PERL_PATH              = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool   
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS         = YES
+MSCGEN_PATH            = 
+HIDE_UNDOC_RELATIONS   = YES
+HAVE_DOT               = YES
+CLASS_GRAPH            = YES
+COLLABORATION_GRAPH    = YES
+GROUP_GRAPHS           = YES
+UML_LOOK               = NO
+TEMPLATE_RELATIONS     = NO
+INCLUDE_GRAPH          = NO
+INCLUDED_BY_GRAPH      = NO
+CALL_GRAPH             = YES
+CALLER_GRAPH           = YES
+GRAPHICAL_HIERARCHY    = YES
+DIRECTORY_GRAPH        = NO
+DOT_IMAGE_FORMAT       = png
+DOT_PATH               = 
+DOTFILE_DIRS           = 
+DOT_GRAPH_MAX_NODES    = 50
+MAX_DOT_GRAPH_DEPTH    = 2
+DOT_TRANSPARENT        = YES
+DOT_MULTI_TARGETS      = NO
+GENERATE_LEGEND        = YES
+DOT_CLEANUP            = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine   
+#---------------------------------------------------------------------------
+SEARCHENGINE           = NO
diff --git a/doc/doxygen_project_name.conf b/doc/doxygen_project_name.conf
new file mode 100644
index 0000000000000000000000000000000000000000..31a95bf9a1533fc7136d51617e1ca82e01701d54
--- /dev/null
+++ b/doc/doxygen_project_name.conf
@@ -0,0 +1 @@
+PROJECT_NAME = "vision_utils"
diff --git a/doc/main.dox b/doc/main.dox
new file mode 100644
index 0000000000000000000000000000000000000000..a71f83243846a0f1b968b173ca5055d0352f0ef3
--- /dev/null
+++ b/doc/main.dox
@@ -0,0 +1,106 @@
+/*! \mainpage vision_utils
+
+  \section Introduction 
+
+  \subsection Pre-Requisites
+
+  This package requires of the following libraries and packages
+	 - <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
+	 - <A href="http://www.doxygen.org">doxygen</a> and 
+	   <A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
+         - stdc++, 
+	 .
+  
+  Under linux all of these utilities are available in ready-to-use packages.
+
+  Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
+
+  \subsection Compilation
+
+  Just download this package, uncompress it, and execute
+   - cd build
+   - cmake ..
+   .
+  to generate the makefile and then
+   - make
+   .
+  to obtain the shared library and also all the example programs.
+
+  The <em>cmake</em> only need to be executed once (make will automatically call
+  <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
+
+  To generate this documentation type
+       - make doc
+       .
+
+  The files in the <em>build</em> directory are genetated by <em>cmake</em>
+  and <em>make</em> and can be safely removed.
+  After doing so you will need to call cmake manually again.
+
+  \subsection Configuration
+
+  The default build mode is DEBUG. That is, objects and executables 
+  include debug information.
+
+  The RELEASE build mode optimizes for speed. To build in this mode
+  execute
+      - cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+      .
+  The release mode will be kept until next time cmake is executed.
+
+  \subsection Installation
+
+  In order to be able to use the library, it it necessary to copy it into the system.
+  To do that, execute
+
+      - make install
+      .
+
+  as root and the shared libraries will be copied to <em>/usr/local/lib/vision_utils</em> directory
+  and the header files will be copied to <em>/usr/local/include/vision_utils</em> directory. At 
+  this point, the library may be used by any user.
+
+  To remove the library from the system, exceute
+      - make uninstall
+      .
+  as root, and all the associated files will be removed from the system.
+
+  \section Customization
+
+  To build a new application using these library, first it is necessary to locate if the library
+  has been installed or not using the following command
+
+      - FIND_PACKAGE(vision_utils REQUIRED)
+
+  In the case that the package is present, it is necessary to add the header files directory to
+  the include directory path by using
+
+      - INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
+
+  Finally, it is also nevessary to link with the desired libraries by using the following command
+
+      - TARGET_LINK_LIBRARIES(<executable name> ${vision_utils_LIBRARY})
+      .
+
+  \section License
+
+  This package is licensed under a  
+  <a href="http://www.gnu.org/licenses/lgpl.html">
+    LGPL 3.0 License</a>.
+
+  \section Disclaimer
+
+   This is free software: you can redistribute it and/or modify
+   it under the terms of the GNU Lesser General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU Lesser General Public License for more details.
+
+   You should have received a copy of the GNU Lesser General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>. 
+
+ */
diff --git a/lib/.gitignore b/lib/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..94548af5beba7825284af746324c8dc5b2f1ea31
--- /dev/null
+++ b/lib/.gitignore
@@ -0,0 +1,3 @@
+*
+*/
+!.gitignore
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7eef9dc97b86120262fd4986ba28410776c926f4
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,44 @@
+# library source files
+SET(sources vision_utils.cpp cam_utils/cam_utils.cpp feature_detector/feature_detector.cpp feature_descriptor/feature_descriptor.cpp feature_matcher/feature_matcher.cpp)
+
+# application header files
+SET(headers vision_utils.h common/vu_base.h cam_utils/cam_utils.h feature_detector/feature_detector.h feature_descriptor/feature_descriptor.h feature_matcher/feature_matcher.h)
+
+# locate the necessary dependencies
+FIND_PACKAGE(Eigen3 REQUIRED)
+FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(Boost REQUIRED)
+
+if (OpenCV_FOUND) 
+  if (${OpenCV_VERSION_MAJOR} GREATER 2)
+    message("-- [INFO] Found OpenCV ${OpenCV_VERSION_MAJOR}.${OpenCV_VERSION_MINOR} support")
+    ADD_DEFINITIONS(-DHAVE_OPENCV_H)
+    SET(USE_CV true)
+  else(${OpenCV_VERSION_MAJOR} GREATER 2)
+    message("[WARN] OpenCV support not installed. Minimum 3.0 version required.")
+    message("[WARN] Current version ${OpenCV_VERSION_MAJOR}")    
+  endif(${OpenCV_VERSION_MAJOR} GREATER 2)
+else(OpenCV_FOUND)
+    message("[WARN] OpenCV support not installed. Minimum 3.0 version required.")
+endif(OpenCV_FOUND)
+
+
+# add the necessary include directories
+INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+
+# create the shared library
+ADD_LIBRARY(vision_utils SHARED ${sources})
+
+# link necessary libraries
+TARGET_LINK_LIBRARIES(vision_utils ${OpenCV_LIBS} ${Boost_LIBRARIES}) 
+
+# install 
+INSTALL(TARGETS vision_utils
+        RUNTIME DESTINATION bin
+        LIBRARY DESTINATION lib/vision_utils
+        ARCHIVE DESTINATION lib/vision_utils)
+INSTALL(FILES ${headers} DESTINATION include/vision_utils)
+INSTALL(FILES ../Findvision_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+
+# examples of usage
+ADD_SUBDIRECTORY(examples)
diff --git a/src/cam_utils/cam_utils.cpp b/src/cam_utils/cam_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d57f926b54c23a59767d5f6de404f76e1dd140d2
--- /dev/null
+++ b/src/cam_utils/cam_utils.cpp
@@ -0,0 +1,49 @@
+#include "cam_utils.h"
+
+CCamUtils::CCamUtils()
+{
+}
+
+CCamUtils::~CCamUtils()
+{
+}
+
+bool CCamUtils::openCamera(const int& _cam_num, cv::VideoCapture& _cam)
+{
+    cv::VideoCapture camera(_cam_num);
+    if (!camera.isOpened())  // check if we succeeded
+    {
+        std::cerr << "ERROR: Could not open camera: " << _cam_num << std::endl;
+        return false;
+    }
+    _cam = camera;
+    return true;
+}
+
+bool CCamUtils::getFrame(cv::VideoCapture& _cam, cv::Mat& _frame)
+{
+    try
+    {
+        _cam >> _frame;
+    }
+    catch (cv::Exception& e)
+    {
+        std::cout << "An exception occurred. Ignoring frame. " << e.err << std::endl;
+        return false;
+    }
+    return true;
+}
+
+bool CCamUtils::showFrame(const std::string& _window_name, const cv::Mat& _frame)
+{
+    try
+    {
+        cv::imshow(_window_name, _frame);
+    }
+    catch (cv::Exception& e)
+    {
+        std::cout << "An exception occurred. Ignoring frame. " << e.err << std::endl;
+        return false;
+    }
+    return true;
+}
diff --git a/src/cam_utils/cam_utils.h b/src/cam_utils/cam_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c06fd44e520e57f654c981f2842519f7be71eb1
--- /dev/null
+++ b/src/cam_utils/cam_utils.h
@@ -0,0 +1,69 @@
+#ifndef _CAMUTILS_H
+#define _CAMUTILS_H
+
+// std stuff
+#include <stdio.h>
+#include <iostream>
+
+// OpenCV stuff
+#include "opencv2/opencv.hpp"
+
+class CCamUtils
+{
+    public:
+
+        /**
+         * \brief Constructor
+         *
+         * Main class constructor.
+         */
+        CCamUtils();
+
+        /**
+         * \brief Destructor
+         *
+         * Main class destructor.
+         */
+        ~CCamUtils();
+
+        /**
+         * \brief Open Webcam
+         *
+         * This method opens the specified webcam using OpenCV
+         * Inputs:
+         * _cam_num: system camera number (int)
+         * Ouptuts:
+         * cam: camera handle (cv::VideoCapture)
+         *
+         * Returns true if the camera is correctly opened
+         *
+         */
+        bool openCamera(const int& _cam_num, cv::VideoCapture& _cam);
+
+        /**
+         * \brief Get frame
+         *
+         * This method gets a frame from the specified webcam
+         *
+         * Inputs:
+         * cam: camera handle (cv::VideoCapture)
+         * Outputs:
+         * frame: filled frame (cv::Mat)
+         * Returns true if the frame is correctly obtained
+         */
+        bool getFrame(cv::VideoCapture& _cam, cv::Mat& _frame);
+
+        /**
+         *\brief Show Frame
+         *
+         * This method shows the specified frame using OpenCV
+         *
+         * Inputs:
+         * window_name: Window name inwhich the frame will be displayed (string)
+         * frame: Frame to be displayed (cv::Mat)
+         * Returns true if the frame is correctly displayed
+         */
+        bool showFrame(const std::string& _window_name, const cv::Mat& _frame);
+};
+
+#endif
diff --git a/src/common/factory.h b/src/common/factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe451fdbcb272a04c899a52b7bfd342e3224f463
--- /dev/null
+++ b/src/common/factory.h
@@ -0,0 +1,330 @@
+#ifndef FACTORY_H_
+#define FACTORY_H_
+
+// wolf
+//#include "wolf.h"
+
+// std
+#include <string>
+#include <map>
+#include <iostream>
+#include <iomanip>
+
+namespace cv_utils
+{
+
+/** \brief Singleton template factory
+ *
+ * This class implements a generic factory as a singleton.
+ *
+ * > IMPORTANT: This template factory can be used to construct many different objects except:
+ * >   - Objects deriving from DetectorBase --> see DetectorFactory
+ * >   - Objects deriving from DescriptorBase --> see DescriptorFactory
+ * >   - Objects deriving from MatcherBase --> see MatcherFactory
+ * >
+ * > The reason for this is that the two cases above need a more elaborated API than the one in this template class.
+ *
+ * \param TypeBase          base type of all the objects created by the factory
+ * \param TypeInput         type of the input argument. Typical cases are std::string for file names, and YAML::Node for YAML nodes.
+ *
+ * - The class is templatized on the class of the produced objects, __TypeBase__.
+ *   The produced objects are always of a class deriving from TypeBase.
+ *   The returned data is always a pointer to TypeBase.
+ *
+ *   For example, you may use as __TypeBase__ the following types:
+ *     - DetectorBase: the Factory creates detectors deriving from DetectorBase and returns base pointers ````DetectorBasePtr```` to them
+ *     - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them.
+ *
+ * - The class in also templatized on the type of the input parameter of the creator, __TypeInput__:
+ *   - ````std::string```` is used when the input parameter is a file name from which to read data (typically a YAML file).
+ *   - ````YAML::Node```` is used when the input parameter is a YAML node with structured data.
+ *
+ * ### Operation of the factory
+ *
+ * #### Rationale
+ *
+ * This factory can create objects of classes deriving from TypeBase.
+ *
+ * > For example, if you want to make a Detector factory, set TypeBase = DetectorBase.\n
+ * > Then, the factory will create specific detectors deriving from DetectorBase.\n
+ * > The specific type of detector (e.g. FAST, SIFT, ORB, etc) is indicated by a string that we call TYPE in this documentation.
+ *
+ * Specific object creation is invoked by the method ````create(TYPE, params ... )````, where
+ *   - the TYPE of object to create is identified with a string
+ *   - the params may be provided in different forms -- see TypeInput.
+ *
+ * The methods to create specific objects are called __creators__.
+ * Creators must be registered to the factory before they can be invoked for object creation.
+ *
+ * This documentation shows you how to:
+ *   - Define correct TYPE names
+ *   - Access the factory
+ *   - Write object creators
+ *   - Register and unregister object creators to the factory
+ *   - Create objects using the factory
+ *   - Examples: Write and register a detector creator for FAST.
+ *
+ * #### Define correct TYPE names
+ * The rule to make new TYPE strings unique is that you skip the generic 'Type' prefix from your class name,
+ * and you build a string in CAPITALS with space separators, e.g.:
+ *   - ParamsDetectorORB -> ````"ORB PARAMS"````
+ *   - ParamsMatcherKnn -> ````"KNN PARAMS"````
+ *   - etc.
+ *
+ * #### Access the factory
+ * The first thing to know is that we have defined typedefs for the templates that we are using. For example:
+ *
+ * \code
+ * typedef Factory<ParamsBase, std::string>        ParamsFactory;
+ * typedef Factory<DetectorBase, std::string>   DetectorFactory;
+ * typedef Factory<DescriptorBase, YAML::Node>           DescriptorFactory;
+ * \endcode
+ *
+ *
+ * Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
+ * To access it, use the static method get(),
+ *
+ *     \code
+ *     Factory<MyTypeBase, MyTypeInput>::get()
+ *     \endcode
+ *
+ * where, of course, you better make use of the appropriate typedef in place of ````Factory<MyTypeBase, MyTypeInput>````.
+ *
+ * You can then call the methods you like, e.g. to create a detector, you use:
+ *
+ *     \code
+ *     DetectorFactory::get().create(...); // see below for creating objects ...
+ *     \endcode
+ *
+ * #### Write creator methods (in your derived object classes)
+ * The method DetectorORB::create(...) exists in the DetectorORB class as a static method.
+ * All these ````XxxXxx::create()```` methods need to have exactly the same API, regardless of the object type.
+ * The API puts into play the two template parameters:
+ *
+ * \code
+ * static TypeBase* create( const TypeInput& );
+ * \endcode
+ *
+ * This API includes an element of type TypeInput, which might be either a std::string, or a YAML::node:
+ *   - ````std::string```` is used to indicate the name of a configuration file. These files are usually YAML files containing configuration data to create your object.
+ *   - ````YAML::Node```` is used to access parts of a YAML file already encoded as nodes, such as when loading landmarks from a SLAM map stored as a YAML file.
+ *
+ *
+ * Two examples:
+ *
+ *      \code
+ *      static ParamsBasePtr create(const std::string& _params_dot_yaml)
+ *      static DetectorBasePtr   create(const YAML::Node& _lmk_yaml_node)
+ *      \endcode
+ *
+ * See further down for an implementation example.
+ *
+ * #### Register object creators
+ * Prior to invoking the creation of an object of a particular TYPE,
+ * you must register the creator for this type into the factory.
+ *
+ * Registering object creators into the factory is done through registerCreator().
+ * You provide an object TYPE string (above), and a pointer to a static method
+ * that knows how to create your specific object, e.g.:
+ *
+ *     \code
+ *     DetectorFactory::get().registerCreator("DETECTOR ORB", DetectorORB::create);
+ *     \endcode
+ *
+ * #### Automatic registration
+ * Currently, registering is performed in specific source files, object_xxxx.cpp.
+ * For example, in detector_orb_yaml.cpp we find the line:
+ *
+ *     \code
+ *     const bool registered_detector_params = ParamsFactory::get().registerCreator("ORB PARAMS", createORBParams);
+ *     \endcode
+ *
+ * which is a static invocation (i.e., it is placed at global scope outside of the ORBParams class).
+ *
+ * Therefore, at application level, all objects that have a .cpp file compiled are automatically registered.
+ *
+ * #### Unregister object creators
+ * The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of the object type.
+ *
+ *     \code
+ *     Factory<MyTypeBase, MyTypeInput>::get().unregisterCreator("CAMERA");
+ *     \endcode
+ *
+ * #### Create objects using the factory
+ * Note: Prior to invoking the creation of a object of a particular type,
+ * you must register the creator for this type into the factory.
+ *
+ * To create e.g. a DetectorORB from a YAML node you type:
+ *
+ *     \code
+ *     DetectorBasePtr det_orb_ptr = Factory<DetectorBasePtr, YAML::Node>::get().create("DETECTOR ORB", det_orb_yaml_node);
+ *     \endcode
+ *
+ * or even better, make use of the convenient typedefs:
+ *
+ *     \code
+ *     DetectorBasePtr det_orb_ptr = DetectorFactory::get().create("DETECTOR ORB", det_orb_yaml_node);
+ *     \endcode
+ *
+ * ### Examples
+ * TODO: Update this
+ * #### Example 1: Writing the creator of DetectorORB from a YAML node
+ *
+ * You can find this code in the detector_orb.cpp file.
+ *
+ * \code
+ *  // Creator (this method is static):
+ * DetectorBasePtr DetectorORB::create(const YAML::Node& _det_orb_node)
+ * {
+ *    // Parse YAML node with lmk info and data
+ *    unsigned int      id              = _det_orb_node["id"].as<unsigned int>();
+ *    int               first_id        = _det_orb_node["first_id"].as<int>();
+ *    bool              first_defined   = _det_orb_node["first_defined"].as<bool>();
+ *    bool              last_defined    = _det_orb_node["last_defined"].as<bool>();
+ *    unsigned int      npoints         = _det_orb_node["points"].size();
+ *    Eigen::MatrixXs   points(2,npoints);
+ *    for (unsigned int i = 0; i < npoints; i++)
+ *    {
+ *        points.col(i)                 = _lmk_node["points"][i].as<Eigen::Vector2s>();
+ *    }
+ *
+ *    // Create a new landmark
+ *    LandmarkBasePtr lmk_ptr = new LandmarkPolyline2D(points, first_defined, last_defined, first_id);
+ *    lmk_ptr->setId(id);
+ *
+ *    return lmk_ptr;
+ * }
+ * \endcode
+ *
+ * #### Example 2: Registering the creator of LandmarkPolyline2D from a YAML node
+ *
+ * You can find this code in the landmark_polyline_2D.cpp file.
+ *
+ * \code
+ * // Register landmark creator (put the register code inside an unnamed namespace):
+ *  namespace
+ *  {
+ *  const bool registered_lmk_polyline_2D = LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
+ *  }
+ *
+ * \endcode
+ *
+ * ### More information
+ *  - IntrinsicsFactory: typedef of this template to create intrinsic structs deriving from IntrinsicsBase directly from YAML files.
+ *  - ProcessorParamsFactory: typedef of this template to create processor params structs deriving from ProcessorParamsBase directly from YAML files.
+ *  - LandmarkFactory: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML nodes.
+ *  - Problem::loadMap() : to load a maps directly from YAML files.
+ *  - You can also check the code in the example file ````src/examples/test_map_yaml.cpp````.
+ *
+ * #### See also
+ *  - SensorFactory: to create sensors
+ *  - ProcessorFactory: to create processors.
+ *  - Problem::installSensor() : to install sensors in WOLF Problem.
+ *  - Problem::installProcessor() : to install processors in WOLF Problem.
+ *
+ */
+template<class TypeBase, typename... TypeInput>
+class Factory
+{
+        typedef std::shared_ptr<TypeBase> TypeBasePtr;
+    public:
+        // example of creator callback (see typedefs below)
+        typedef TypeBasePtr (*CreatorCallback)(TypeInput... _input);
+
+        // Main factory API
+        bool registerCreator(const std::string& _type, CreatorCallback createFn);
+        bool unregisterCreator(const std::string& _type);
+        TypeBasePtr create(const std::string& _type, TypeInput... _input);
+        std::string getClass();
+
+    private:
+
+        typedef std::map<std::string, CreatorCallback> CallbackMap;
+
+        CallbackMap callbacks_;
+
+        // Singleton ---------------------------------------------------
+        // This class is a singleton. The code below guarantees this.
+        // See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
+    public:
+        static Factory& get();
+        Factory(const Factory&)         = delete;
+        void operator=(Factory const&)  = delete;
+    private:
+        Factory()  { }
+        ~Factory() { }
+};
+
+template<class TypeBase, typename... TypeInput>
+inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
+{
+    bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
+    if (reg)
+        std::cout << std::setw(22) << std::left << getClass() << " <--  registered  " << _type << std::endl;
+    else
+        std::cout << std::setw(22) << std::left << getClass() << " X--  skipping  " << _type << ": already registered." << std::endl;
+
+    return reg;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
+{
+    return callbacks_.erase(_type) == 1;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
+{
+    typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
+
+    if (creator_callback_it == callbacks_.end())
+        // not found
+        throw std::runtime_error(getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
+
+    // Invoke the creation function
+    return (creator_callback_it->second)(std::forward<TypeInput>(_input)...);
+}
+
+template<class TypeBase, typename... TypeInput>
+inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
+{
+    static Factory instance_;
+    return instance_;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline std::string Factory<TypeBase, TypeInput...>::getClass()
+{
+    return "Factory<class TypeBase>";
+}
+
+// Some specializations
+//======================
+
+// Parameters
+struct ParamsBase;
+typedef Factory<ParamsBase,
+        const std::string&> ParamsFactory;
+template<>
+inline std::string ParamsFactory::getClass()
+{
+    return "ParamsFactory";
+}
+
+// Frames
+class FrameBase;
+class TimeStamp;
+typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
+template<>
+inline std::string FrameFactory::getClass()
+{
+    return "FrameFactory";
+}
+#define WOLF_REGISTER_FRAME(FrameType, FrameName) \
+  namespace{ const bool FrameName##Registered = \
+    FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
+
+} /* namespace cv_utils */
+
+#endif /* FACTORY_H_ */
diff --git a/src/common/vu_base.h b/src/common/vu_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..b35c0595b7420ffac51af40bda4b98ce30ace379
--- /dev/null
+++ b/src/common/vu_base.h
@@ -0,0 +1,217 @@
+#ifndef _VU_BASE_H
+#define _VU_BASE_H
+
+// std
+#include <functional>
+#include <set>
+#include <iostream>
+#include <map>
+#include <vector>
+#include <string>
+#include <iostream>
+#include <time.h>
+
+// Boost
+#include <boost/assign/std/vector.hpp> // for 'operator+=()'
+using namespace boost::assign; // bring 'operator+=()' into scope
+
+// OpenCV
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/xfeatures2d/nonfree.hpp>
+#include <opencv2/xfeatures2d.hpp>
+#include <opencv2/line_descriptor/descriptor.hpp>
+
+typedef std::vector<cv::KeyPoint> KeyPointVector;
+typedef std::vector<cv::KeyPoint> KeyPointVector;
+typedef std::vector<cv::line_descriptor::KeyLine> KeyLineVector;
+
+/**
+ * \brief Class to set object types
+ */
+class CTypes {
+
+public:
+
+	CTypes(void): init_(false) {};
+
+	~CTypes(void) {};
+
+	void set(std::vector<std::string> &_types)
+	{
+		for (unsigned int ii = 0; ii < _types.size(); ii++)
+		    types_[_types[ii]] = ii;
+		init_= true;
+	};
+
+	std::vector<std::string> list(void)
+	{
+	        std::vector<std::string> list;
+	        list.reserve(types_.size());
+        	for(std::map<std::string,int>::iterator it = types_.begin(); it != types_.end(); ++it)
+        	{
+       			list.push_back(it->first);
+        	}
+	        return list;
+	}
+
+	int operator()(const std::string &name) {
+		if (init_)
+		{
+			// Search done to take advantage of MAP alphabetical order
+			int idx = 0;
+			for (std::map<std::string, int>::const_iterator it = types_.begin();it != types_.end(); ++it)
+			{
+				if (it->first.compare(name) == 0)
+					return idx;
+				++idx;
+			}
+
+
+		}
+		else
+		{
+			std::cerr << "[Vision Utils]: Wrong object initialization" << std::cout;
+		}
+		return 0;
+	};
+
+	std::string operator()(const int &num) {
+		if (init_)
+		{
+			// Search done to take advantage of MAP alphabetical order
+			int idx = 0;
+			for (std::map<std::string, int>::const_iterator it = types_.begin();it != types_.end(); ++it)
+			{
+				if (num == idx)
+					return it->first;
+				++idx;
+			}
+		}
+		else
+			std::cerr << "[Vision Utils]: Wrong object initialization" << std::cout;
+		return 0;
+	};
+
+	unsigned int size(void)
+	{ return types_.size(); }
+
+protected:
+
+	std::map<std::string, int> types_;
+	bool init_;
+};
+
+/**
+ * \brief Class to set object parameters
+ */
+class CParams {
+public:
+	CParams(void) {
+	}
+	;
+	~CParams(void) {
+	}
+	;
+};
+
+
+/**
+ * \brief Superbase Class
+ *
+ * This Super class is created in order to work with pointers to the derived
+ * classes. Otherwise CSensor_base is templated and does not allow it.
+ */
+class CVu_Superbase
+{
+public:
+  CVu_Superbase(){};
+  virtual ~CVu_Superbase(){};
+
+};
+
+/**
+ * \brief base Class
+ */
+template <class T_PARAMS>
+class CVu_Base: public CVu_Superbase {
+
+public:
+
+	CVu_Base(void): is_init_(false) {};
+
+	virtual ~CVu_Base(void){};
+
+	/**
+	 * \brief Get detection duration
+	 */
+	double getTime(void){return comp_time_;};
+
+	/**
+	 * \brief list types
+	 */
+	std::vector<std::string> list(void)
+	{ 	return types_.list();};
+
+	/**
+	 * \brief Set object without constructor
+	 */
+	bool set(const std::string& _type, const T_PARAMS &_params)
+	{
+	    bool success = init(_type, _params);
+	    return success;
+	};
+
+	void get(std::string& _type)
+	{	_type = type_; };
+
+	void get(int& _type)
+	{
+		_type = types_(type_);
+	};
+
+	cv::Mat drawKeyFeatures(const cv::Mat& _image, const KeyPointVector& _kp_vec)
+	{
+	    cv::Mat img_out(_image);
+
+	    for (unsigned int ii = 0; ii < _kp_vec.size(); ++ii)
+	        cv::circle(img_out, _kp_vec[ii].pt, 5, cv::Scalar(128, 128, 255), -1);
+
+	    return img_out;
+	}
+
+	cv::Mat drawKeyFeatures(const cv::Mat& _image, const KeyLineVector& _kl_vec)
+	{
+	    cv::Mat img_out(_image);
+
+	    for (unsigned int ii = 0; ii < _kl_vec.size(); ++ii)
+	        cv::line(img_out, _kl_vec[ii].getStartPoint(), _kl_vec[ii].getEndPoint(), cv::Scalar(128, 128, 255), 3);
+
+	    return img_out;
+	}
+
+
+protected:
+
+	// Flags
+	bool is_init_;
+
+	double comp_time_; // Detection time
+
+	CTypes types_;
+
+	std::string type_;
+
+	virtual bool init(const std::string &_type, const T_PARAMS &_params) = 0;
+
+    /**
+     * \brief Set all types
+     */
+	virtual void setAllTypes(void) = 0;
+
+};
+
+#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..699b1161afd320eae55fc39dbaf66d540abffabb
--- /dev/null
+++ b/src/examples/CMakeLists.txt
@@ -0,0 +1,11 @@
+# create an example application
+ADD_EXECUTABLE(capture capture.cpp)
+ADD_EXECUTABLE(feature_detectors feature_detect.cpp)
+ADD_EXECUTABLE(feature_detectors_and_descriptors feature_detect_descript.cpp)
+ADD_EXECUTABLE(feature_detectors_descriptors_and_matchers feature_detect_descript_match.cpp)
+
+# link necessary libraries
+TARGET_LINK_LIBRARIES(capture vision_utils)
+TARGET_LINK_LIBRARIES(feature_detectors vision_utils)
+TARGET_LINK_LIBRARIES(feature_detectors_and_descriptors vision_utils)
+TARGET_LINK_LIBRARIES(feature_detectors_descriptors_and_matchers vision_utils)
\ No newline at end of file
diff --git a/src/examples/capture.cpp b/src/examples/capture.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce2601496a3683ebba4f8f2ee9ebea07f39a0549
--- /dev/null
+++ b/src/examples/capture.cpp
@@ -0,0 +1,29 @@
+// OWN stuff
+#include <string>
+
+#include <vision_utils.h>
+
+int main(int argc, char *argv[])
+{
+    // Open camera
+    cv::VideoCapture cam;
+    CCamUtils cam_fc;
+    cam_fc.openCamera(0, cam);
+
+    cv::startWindowThread();
+    cv::namedWindow("Cam Test", 1);
+
+    for (;;)
+    {
+        // Get frame
+        cv::Mat frame;
+        cam_fc.getFrame(cam, frame);
+
+        // Show frame
+        cv::imshow("Cam Test", frame);
+        cv::waitKey(1);
+        //if (cv::waitKey(30) >= 0) break;
+    }
+
+    cv::destroyAllWindows();
+}
diff --git a/src/examples/feature_detect.cpp b/src/examples/feature_detect.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dae60b028c05cb5f55133d039e344dcc50b4e16
--- /dev/null
+++ b/src/examples/feature_detect.cpp
@@ -0,0 +1,90 @@
+// OWN stuff
+#include <string>
+
+#include "vision_utils.h"
+
+
+int main(int argc, char *argv[])
+{
+    std::cout << "----------------------------------------" << std::endl;
+    std::cout << "|       Feature detector example       |" << std::endl;
+    std::cout << "----------------------------------------" << std::endl;
+    std::cout << std::endl;
+
+    CFeature_Detector detector;
+    CDetector_Params detector_params; // TODO: Fill parameters
+
+    std::vector<std::string> detectors_list;
+    detectors_list = detector.list();
+
+    for (unsigned int ii=0; ii<detectors_list.size(); ii++)
+        std::cout << "[" << ii << "]: " << detectors_list[ii] << std::endl;
+
+    //***********
+    // Detector |
+    //***********
+
+    // Get default value
+    int feat_type;
+    detector.get(feat_type);
+
+    std::cout << std::endl << "Which DETECTOR do you want to test?[default: " <<  detectors_list[feat_type] << "]";
+
+    feat_type = readFromUser(feat_type);
+
+    detector.set(detectors_list[feat_type],detector_params);
+
+    std::cout << std::endl << "Testing: " << detectors_list[feat_type] << std::endl;
+
+    // *****************************
+
+    // Open camera
+    cv::VideoCapture cam;
+    CCamUtils cam_fc;
+    cam_fc.openCamera(0, cam);
+
+    // Create displays
+    cv::startWindowThread();
+    cv::namedWindow("Original image", cv::WINDOW_NORMAL);
+    cv::namedWindow("Detections", cv::WINDOW_NORMAL);
+
+    // The following line is used to remove the OpenCV "init done" from the terminal
+    std::cout << "\e[A" << "         " << std::endl;
+
+    for (int nframe = 0; nframe < 1000; ++nframe)
+    {
+        // Get frame
+        cv::Mat frame;
+        cam_fc.getFrame(cam, frame);
+
+        // Show ORIGINAL frame
+        cv::imshow("Original image", frame);
+
+        if (!detector.isLine())
+        {
+            KeyPointVector keypoints;
+            keypoints = detector.detectKeyPoints(frame);
+            // Show frame with features
+            detector.drawKeyFeatures(frame, keypoints);
+        }
+        else
+        {
+            KeyLineVector keypoints;
+            keypoints = detector.detectKeyLines(frame);
+            // Show frame with features
+            detector.drawKeyFeatures(frame, keypoints);
+        }
+
+        std::cout << "\e[A" << "Detection time: " << detector.getTime() << std::endl;
+
+        // Show NEW frame
+        cv::imshow("Detections", frame);
+        cv::waitKey(1);
+        // if (cv::waitKey(30) >= 0) break;
+    }
+
+    cv::destroyAllWindows();
+}
+
+
+
diff --git a/src/examples/feature_detect_descript.cpp b/src/examples/feature_detect_descript.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4ba26456b2b65603bd0ebe9db770286ad7400b9
--- /dev/null
+++ b/src/examples/feature_detect_descript.cpp
@@ -0,0 +1,115 @@
+// OWN stuff
+#include <string>
+
+#include "vision_utils.h"
+
+
+int main(int argc, char *argv[])
+{
+    std::cout << "---------------------------------------------------------" << std::endl;
+    std::cout << "|       Feature detectors and descriptors example       |" << std::endl;
+    std::cout << "---------------------------------------------------------" << std::endl;
+    std::cout << std::endl;
+
+    //***********
+    // Detector |
+    //***********
+
+    CFeature_Detector detector;
+    CDetector_Params detector_params; // TODO: Fill parameters
+
+    std::vector<std::string> detectors_list;
+    detectors_list = detector.list();
+
+    for (unsigned int ii=0; ii<detectors_list.size(); ii++)
+    {
+    	if ( (detectors_list[ii].compare("LSD")!=0) && (detectors_list[ii].compare("ED")!=0) )
+    		std::cout << "[" << ii << "]: " << detectors_list[ii] << std::endl;
+    }
+
+    // Get default value
+    int feat_type;
+    detector.get(feat_type);
+
+    std::cout << std::endl << "Which DETECTOR do you want to test?[default: " <<  detectors_list[feat_type] << "]";
+
+    feat_type = readFromUser(feat_type);
+
+    detector.set(detectors_list[feat_type],detector_params);
+
+    //*************
+    // Descriptor |
+    //*************
+
+    CFeature_Descriptor descriptor;
+    CDescriptor_Params descriptor_params; // TODO: Fill parameters
+
+    std::vector<std::string> descriptor_list;
+    descriptor_list = descriptor.list();
+
+    for (unsigned int ii=0; ii<descriptor_list.size(); ii++)
+        std::cout << "[" << ii << "]: " << descriptor_list[ii] << std::endl;
+
+    // Get default value
+    int desc_type;
+    descriptor.get(desc_type);
+
+    std::cout << std::endl << "Which DESCRIPTOR do you want to test?[default: " <<  descriptor_list[desc_type] << "]";
+
+    desc_type = readFromUser(desc_type);
+
+    descriptor.set(descriptor_list[desc_type],descriptor_params);
+
+    std::cout << std::endl << "Testing: " << detectors_list[feat_type] << " + " << descriptor_list[desc_type] << " (DETECTOR + DESCRIPTOR)" << std::endl;
+
+    // *****************************
+
+    // Open camera
+    cv::VideoCapture cam;
+    CCamUtils cam_fc;
+    cam_fc.openCamera(0, cam);
+
+    // Create displays
+    cv::startWindowThread();
+    cv::namedWindow("Original image", cv::WINDOW_NORMAL);
+    cv::namedWindow("Detections", cv::WINDOW_NORMAL);
+
+    // The following line is used to remove the OpenCV "init done" from the terminal
+    std::cout << "\e[A" << "         " << std::endl;
+
+    for (int nframe = 0; nframe < 1000; ++nframe)
+    {
+        // Get frame
+        cv::Mat frame;
+        cam_fc.getFrame(cam, frame);
+
+        // Show ORIGINAL frame
+        cv::imshow("Original image", frame);
+
+      	// Detector
+        KeyPointVector keypoints;
+        keypoints = detector.detectKeyPoints(frame);
+
+        // Descriptor
+        cv::Mat descriptors;
+        descriptors = descriptor.getDescriptor(frame,keypoints);
+
+        // Show frame with features
+        detector.drawKeyFeatures(frame, keypoints);
+
+        std::cout << "\e[A" <<
+        		"Detection time: " << detector.getTime() << "    " <<
+				"Description time: " << descriptor.getTime() << "    " <<
+        		"TOTAL time: " << detector.getTime() + descriptor.getTime() << std::endl;
+
+        // Show NEW frame
+        cv::imshow("Detections", frame);
+        cv::waitKey(1);
+        // if (cv::waitKey(30) >= 0) break;
+    }
+
+    cv::destroyAllWindows();
+}
+
+
+
diff --git a/src/examples/feature_detect_descript_match.cpp b/src/examples/feature_detect_descript_match.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..407643d57364e291cccbaf862fb7f3c1c7c7daa9
--- /dev/null
+++ b/src/examples/feature_detect_descript_match.cpp
@@ -0,0 +1,201 @@
+// OWN stuff
+#include <string>
+
+#include "vision_utils.h"
+#include <sstream>
+
+int main(int argc, char *argv[])
+{
+    std::cout << "-------------------------------------------------------------------" << std::endl;
+    std::cout << "|       Feature detectors, descriptors and matchers example       |" << std::endl;
+    std::cout << "-------------------------------------------------------------------" << std::endl;
+    std::cout << std::endl;
+
+    //***********
+     // Detector |
+     //***********
+
+     CFeature_Detector detector;
+     CDetector_Params detector_params; // TODO: Fill parameters
+
+     std::vector<std::string> detectors_list;
+     detectors_list = detector.list();
+
+     for (unsigned int ii=0; ii<detectors_list.size(); ii++)
+     {
+     	if ( (detectors_list[ii].compare("LSD")!=0) && (detectors_list[ii].compare("ED")!=0) )
+     		std::cout << "[" << ii << "]: " << detectors_list[ii] << std::endl;
+     }
+
+     // Get default value
+     int feat_type;
+     detector.get(feat_type);
+
+     std::cout << std::endl << "Which DETECTOR do you want to test?[default: " <<  detectors_list[feat_type] << "]";
+
+//     feat_type = readFromUser(feat_type);
+
+     detector.set(detectors_list[feat_type],detector_params);
+
+     //*************
+     // Descriptor |
+     //*************
+
+     CFeature_Descriptor descriptor;
+     CDescriptor_Params descriptor_params; // TODO: Fill parameters
+
+     std::vector<std::string> descriptor_list;
+     descriptor_list = descriptor.list();
+
+     for (unsigned int ii=0; ii<descriptor_list.size(); ii++)
+         std::cout << "[" << ii << "]: " << descriptor_list[ii] << std::endl;
+
+     // Get default value
+     int desc_type;
+     descriptor.get(desc_type);
+
+     std::cout << std::endl << "Which DESCRIPTOR do you want to test?[default: " <<  descriptor_list[desc_type] << "]" << std::endl;
+
+//     desc_type = readFromUser(desc_type);
+
+     descriptor.set(descriptor_list[desc_type],descriptor_params);
+
+     //**********
+     // Matcher |
+     //**********
+
+     CFeature_Matcher matcher;
+     CMatcher_Params matcher_params; // TODO: Fill parameters
+     CMatcher_Params matcher_search_params; // TODO: Fill parameters
+
+     std::vector<std::string> matchers_list;
+     matchers_list = matcher.list();
+
+     for (unsigned int ii=0; ii<matchers_list.size(); ii++)
+    	 std::cout << "[" << ii << "]: " << matchers_list[ii] << std::endl;
+
+     // Get default value
+     int match_type;
+     matcher.get(match_type);
+
+     std::cout << std::endl << "Which MATCHER do you want to test?[default: " <<  matchers_list[match_type] << "]" << std::endl;
+
+//     match_type = readFromUser(match_type);
+
+     matcher.set(matchers_list[match_type],matcher_params);
+
+     std::vector<std::string> matchers_search_list;
+     matchers_search_list = matcher.listSearchTypes();
+
+     for (unsigned int ii=0; ii<matchers_search_list.size(); ii++)
+    	 std::cout << "[" << ii << "]: " << matchers_search_list[ii] << std::endl;
+
+     // Get default value
+     int match_search_type;
+     matcher.getSearchType(match_search_type);
+
+     std::cout << std::endl << "Which MATCHER SEARCH do you want to test?[default: " <<  matchers_search_list[match_search_type] << "]" << std::endl;
+
+//     match_search_type = readFromUser(match_search_type);
+
+     matcher.setSearchType(matchers_search_list[match_search_type],matcher_search_params);
+
+     std::cout << std::endl << "Testing: " << detectors_list[feat_type] << " + " << descriptor_list[desc_type] << " + " << matchers_list[match_type] << " + " << matchers_search_list[match_search_type] << " (DETECTOR + DESCRIPTOR + MATCHER + MATCHER SEARCH)" << std::endl;
+
+     // *****************************
+
+     // Open camera
+     cv::VideoCapture cam;
+     CCamUtils cam_fc;
+     cam_fc.openCamera(0, cam);
+
+     // Create displays
+     cv::startWindowThread();
+     cv::namedWindow("Original image", cv::WINDOW_NORMAL);
+     cv::namedWindow("Detections", cv::WINDOW_NORMAL);
+     cv::namedWindow("Matches", cv::WINDOW_NORMAL);
+
+     cv::Mat frame_old;
+     KeyPointVector keypoints_old;
+     KeyPointVector good_keypoints;
+     cv::Mat descriptors_old;
+     std::vector<cv::DMatch> good_matches;
+
+     // The following line is used to remove the OpenCV "init done" from the terminal
+     std::cout << "\e[A" << "         " << std::endl;
+
+     for (int nframe = 0; nframe < 1000; ++nframe)
+     {
+        // Get frame
+    	cv::Mat frame;
+        cam_fc.getFrame(cam, frame);
+        cv::Mat frame_matches = frame.clone();
+
+        // Show ORIGINAL frame
+        cv::imshow("Original image", frame);
+
+		// Detector
+        KeyPointVector keypoints;
+        keypoints = detector.detectKeyPoints(frame);
+
+        // Descriptor
+        cv::Mat descriptors;
+        descriptors = descriptor.getDescriptor(frame,keypoints);
+
+        // Matcher
+        if  (nframe > 1)
+        {
+        	// TODO: Implement this object creation depending on user preferences
+        	//     if (matchers_search_list[match_search_type].compare("Match") == 0)
+        	//    	 std::vector<cv::DMatch> matches;
+        	//     else
+        	std::vector< std::vector<cv::DMatch> > matches;
+        	matcher.match(descriptors,descriptors_old,matches);
+
+        	// Filter matches
+        	good_matches.clear();
+        	good_keypoints.clear();
+        	matcher.filterMatches(keypoints_old, keypoints, matches, frame.rows, frame.cols, good_matches, good_keypoints);
+        }
+
+        // Update objects
+        keypoints_old.clear();
+        keypoints_old.resize(keypoints.size());
+        for (unsigned int ii = 0; ii < keypoints.size(); ++ii)
+        	keypoints_old.push_back(keypoints[ii]);
+        descriptors_old = cv::Mat(descriptors.size(),descriptors.type());
+        descriptors_old = descriptors.clone();
+        frame_old = frame;
+
+        // Show frame with features
+        detector.drawKeyFeatures(frame, keypoints);
+
+        // Draw matches
+        if  (nframe > 1 && !keypoints_old.empty() && !keypoints.empty())
+        {
+        	matcher.drawKeyFeatures(frame_matches, good_keypoints);
+//        	cv::drawMatches( frame_old, keypoints_old, frame, keypoints,
+//        			good_matches, frame_matches, cv::Scalar::all(-1), cv::Scalar::all(-1),
+//					std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+        	for (unsigned int ii = 0; ii < good_keypoints.size(); ++ii)
+        		cv::line(frame_matches,good_keypoints[ii].pt,good_keypoints[ii].pt,cv::Scalar(0,255,0),3);
+        	cv::imshow("Matches", frame_matches);
+        }
+
+        // Show NEW frame
+        cv::imshow("Detections", frame);
+        cv::waitKey(1);
+        // if (cv::waitKey(30) >= 0) break;
+
+        std::cout << "\e[A" <<
+        		"Detection time: " << detector.getTime() << "    " <<
+				"Description time: " << descriptor.getTime() << "    " <<
+				"Match time: " << matcher.getTime() << "    " <<
+        		"TOTAL time: " << detector.getTime() + descriptor.getTime() << std::endl;
+     }
+
+    cv::destroyAllWindows();
+}
+
+
+
diff --git a/src/feature_descriptor/feature_descriptor.cpp b/src/feature_descriptor/feature_descriptor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aca2e4049f7074d7d7f5a5fb420f6fc8e429c284
--- /dev/null
+++ b/src/feature_descriptor/feature_descriptor.cpp
@@ -0,0 +1,121 @@
+#include "feature_descriptor.h"
+
+CFeature_Descriptor::CFeature_Descriptor(const std::string& _type, const CDescriptor_Params& _params)
+{
+    setAllTypes();
+
+	is_init_ = init(_type, _params);
+
+	if (!is_init_)
+	{
+		std::cerr
+				<< "[Feature Descriptor]: Something went wrong during initialization! Feature Descriptor not initialized."
+				<< std::endl;
+	}
+}
+
+CFeature_Descriptor::CFeature_Descriptor()
+{
+    setAllTypes();
+}
+
+CFeature_Descriptor::~CFeature_Descriptor() {
+}
+
+void CFeature_Descriptor::setAllTypes(void)
+{
+    // Define all types
+    std::vector<std::string> types;
+    types += "ORB", "SIFT", "SURF", "KAZE", "AKAZE", "BRISK", "LATCH", "FREAK", "BRIEF", "DAISY", "LUCID";
+    types_.set(types);
+    type_ = "ORB"; // Default value
+}
+
+bool CFeature_Descriptor::init(const std::string& _type, const CDescriptor_Params& _params)
+{
+    if (is_init_)
+    {
+        std::cerr << "[CFeature_Descriptor]: Descriptor already initialized." << std::endl;
+        return false;
+    }
+
+    // TODO: Set parameters for each detector type
+    if (_type.compare(types_(0))==0)
+    {
+        feature_descriptor_ = cv::ORB::create();
+        type_ = types_(0);
+    }
+    else if (_type.compare(types_(1))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::SIFT::create();
+        type_ = types_(1);
+    }
+    else if (_type.compare(types_(2))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::SURF::create();
+        type_ = types_(2);
+    }
+    else if (_type.compare(types_(3))==0)
+    {
+        feature_descriptor_ = cv::KAZE::create();
+        type_ = types_(3);
+    }
+    else if (_type.compare(types_(4))==0)
+    {
+        feature_descriptor_ = cv::AKAZE::create();
+        type_ = types_(4);
+    }
+    else if (_type.compare(types_(5))==0)
+    {
+        feature_descriptor_ = cv::BRISK::create();
+        type_ = types_(0);
+    }
+    else if (_type.compare(types_(6))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::LATCH::create();
+        type_ = types_(6);
+    }
+    else if (_type.compare(types_(7))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::FREAK::create();
+        type_ = types_(7);
+    }
+    else if (_type.compare(types_(8))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::BriefDescriptorExtractor::create();
+        type_ = types_(8);
+    }
+    else if (_type.compare(types_(9))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::DAISY::create();
+        type_ = types_(9);
+            }
+    else if (_type.compare(types_(10))==0)
+    {
+        feature_descriptor_ = cv::xfeatures2d::LUCID::create(1,2);
+        type_ = types_(10);
+            }
+    else
+    {
+        std::cerr << "[Feature Descriptor]: descriptor type " << _type << " doesn't exist !" << std::endl;
+        return false;
+    }
+
+    is_init_ = true;
+    return true;
+}
+
+cv::Mat CFeature_Descriptor::getDescriptor(const cv::Mat& _image, KeyPointVector& _kpts)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Descriptor::getDescriptor]: Descriptor non initialized." << std::endl;
+
+    cv::Mat descriptors;
+
+    clock_t tStart = clock();
+    if (!_kpts.empty())
+    	feature_descriptor_->compute(_image, _kpts, descriptors);
+    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+
+    return descriptors;
+}
diff --git a/src/feature_descriptor/feature_descriptor.h b/src/feature_descriptor/feature_descriptor.h
new file mode 100644
index 0000000000000000000000000000000000000000..27cd86676dd7230dbf96a3cffefd70e33b2926e6
--- /dev/null
+++ b/src/feature_descriptor/feature_descriptor.h
@@ -0,0 +1,51 @@
+#ifndef _FEATURE_DESCRIPTOR_H
+#define _FEATURE_DESCRIPTOR_H
+
+#include "../common/vu_base.h"
+
+typedef cv::Ptr<cv::DescriptorExtractor> FeatureDescriptorPtr;
+
+/**
+ * \brief Feature descriptor parameters class
+ */
+class CDescriptor_Params: public CParams {
+public:
+    CDescriptor_Params(void) {
+    }
+    ;
+    ~CDescriptor_Params(void) {
+    }
+    ;
+};
+
+class CFeature_Descriptor: public CVu_Base <CDescriptor_Params>{
+
+public:
+
+	CFeature_Descriptor(const std::string& _type, const CDescriptor_Params& _params);
+	CFeature_Descriptor(void);
+
+	~CFeature_Descriptor(void);
+
+	/**
+	 * \brief Get descriptors
+	 */
+	cv::Mat getDescriptor(const cv::Mat& _image,
+			KeyPointVector& _kpts);
+
+private:
+
+	FeatureDescriptorPtr feature_descriptor_; // Feature point descriptor
+
+    /**
+     * \brief Set all types
+     */
+    void setAllTypes(void);
+
+	/**
+	 * \brief Initialize descriptor
+	 */
+	bool init(const std::string &_type, const CDescriptor_Params &_params);
+};
+
+#endif
diff --git a/src/feature_detector/feature_detector.cpp b/src/feature_detector/feature_detector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9dacdec7fe9bd6d80213185a937893c25a5c369a
--- /dev/null
+++ b/src/feature_detector/feature_detector.cpp
@@ -0,0 +1,202 @@
+#include "feature_detector.h"
+
+CFeature_Detector::CFeature_Detector(const std::string& _type, const CDetector_Params& _params) :
+    is_line_(false), is_limited_(false), keypoints_limit_(-1)
+{
+    setAllTypes();
+
+    is_init_ = init(_type, _params);
+
+    if (!is_init_)
+    {
+        std::cerr << "[Feature Detector]: Something went wrong during initialization! Feature Detector not initialized."
+                << std::endl;
+    }
+}
+
+CFeature_Detector::CFeature_Detector(void) :
+     is_line_(false), is_limited_(false), keypoints_limit_(-1)
+{
+    setAllTypes();
+}
+
+CFeature_Detector::~CFeature_Detector()
+{
+}
+
+void CFeature_Detector::setAllTypes(void)
+{
+    // Define all types
+    std::vector<std::string> types;
+    types += "FAST", "SIFT", "SURF", "ORB", "BRISK", "MSER", "GFTT", "HARRIS", "SBD", "KAZE", "AKAZE", "AGAST", "LSD", "ED";
+    types_.set(types);
+    type_ = "ORB"; // Default value
+}
+
+bool CFeature_Detector::init(const std::string& _type, const CDetector_Params& _params)
+{
+	if (is_init_)
+    {
+        std::cerr << "[CFeature_Detector::init]: Detector already initialized." << std::endl;
+        return false;
+    }
+
+    // TODO: Set parameters for each detector type
+    if (_type.compare(types_(0))==0)
+    {
+    	feature_detector_ = cv::FastFeatureDetector::create();
+    	type_ = types_(0);
+    }
+    else if (_type.compare(types_(1))==0)
+    {
+    	feature_detector_ = cv::xfeatures2d::SIFT::create();
+    	type_ = types_(1);
+    }
+    else if (_type.compare(types_(2))==0)
+    {
+    	feature_detector_ = cv::xfeatures2d::SURF::create();
+    	type_ = types_(2);
+    }
+    else if (_type.compare(types_(3))==0)
+    {
+    	feature_detector_ = cv::ORB::create();
+    	type_ = types_(3);
+    }
+    else if (_type.compare(types_(4))==0)
+    {
+    	feature_detector_ = cv::BRISK::create();
+    	type_ = types_(4);
+    }
+    else if (_type.compare(types_(5))==0)
+    {
+    	feature_detector_ = cv::MSER::create();
+    	type_ = types_(0);
+    }
+    else if (_type.compare(types_(6))==0)
+    {
+    	feature_detector_ = cv::GFTTDetector::create(1000, 0.01, 1.0, 3, false, 0.04);
+    	type_ = types_(6);
+    }
+    else if (_type.compare(types_(7))==0)
+    {
+    	feature_detector_ = cv::GFTTDetector::create(1000, 0.01, 1.0, 3, true, 0.04);
+    	type_ = types_(7);
+    }
+    else if (_type.compare(types_(8))==0)
+    {
+    	feature_detector_ = cv::SimpleBlobDetector::create();
+    	type_ = types_(8);
+    }
+    else if (_type.compare(types_(9))==0)
+    {
+    	feature_detector_ = cv::KAZE::create();
+    	type_ = types_(9);
+    	    }
+    else if (_type.compare(types_(10))==0)
+    {
+    	feature_detector_ = cv::AKAZE::create();
+    	type_ = types_(10);
+    	    }
+    else if (_type.compare(types_(11))==0)
+    {
+    	feature_detector_ = cv::AgastFeatureDetector::create();
+    	type_ = types_(11);
+    	    }
+    else if (_type.compare(types_(12))==0)
+    {
+        lsd_detector_ = cv::line_descriptor::LSDDetector::createLSDDetector();
+        type_ = types_(12);
+            }
+    else if (_type.compare(types_(13))==0)
+    {
+    	ed_detector_ = cv::line_descriptor::BinaryDescriptor::createBinaryDescriptor();
+    	type_ = types_(13);
+    	    }
+    else
+    {
+    	std::cerr << "[Feature Detector]: detector_type " << _type << " doesn't exist !" << std::endl;
+    	return false;
+    }
+
+    is_init_ = true;
+    return true;
+}
+
+bool CFeature_Detector::isLine(void)
+{
+    return is_line_;
+}
+
+bool CFeature_Detector::isLimited(void)
+{
+    return is_limited_;
+}
+
+int CFeature_Detector::getKeyPointsLimit(void)
+{
+    return keypoints_limit_;
+}
+
+KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image)
+{
+	cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
+	return detectKeyPoints(_image, mask);
+}
+
+KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image, const cv::Mat& _mask)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Detector::detectKeyPoints]: Detector non initialized." << std::endl;
+
+    KeyPointVector kpts;
+
+    clock_t tStart = clock();
+    feature_detector_->detect(_image, kpts, _mask);
+    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+
+    if (isLimited())
+    {
+        cv::KeyPointsFilter filter;
+        filter.retainBest(kpts, this->getKeyPointsLimit());
+    }
+
+    return kpts;
+}
+
+KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image)
+{
+	cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
+	return detectKeyLines(_image, mask);
+}
+
+KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image, const cv::Mat& _mask)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Detector::detectKeyLines]: Detector non initialized." << std::endl;
+
+    KeyLineVector kls;
+    KeyLineVector kls2;
+
+    clock_t tStart = clock();
+    if (type_.compare("LSD")==0)
+        lsd_detector_->detect(_image, kls, SCALE_FACTOR_LINE_DETECTOR, NUM_OCTAVE_LINE_DETECTOR, _mask);
+    else
+        ed_detector_->detect(_image, kls2, _mask);
+    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+    kls.insert(kls.end(), kls2.begin(), kls2.end());
+
+    return kls;
+}
+
+bool CFeature_Detector::setLimitKeypts(unsigned int _nFeatures)
+{
+    if (_nFeatures > 0)
+    {
+        keypoints_limit_ = _nFeatures;
+        return is_limited_ = true;
+    }
+
+    return false;
+}
+
+
diff --git a/src/feature_detector/feature_detector.h b/src/feature_detector/feature_detector.h
new file mode 100644
index 0000000000000000000000000000000000000000..fc0d945f90064f91d695244db4ebe7e68f3eb4db
--- /dev/null
+++ b/src/feature_detector/feature_detector.h
@@ -0,0 +1,94 @@
+#ifndef _FEATURE_DETECTOR_H
+#define _FEATURE_DETECTOR_H
+
+#include "../common/vu_base.h"
+
+#define SCALE_FACTOR_LINE_DETECTOR 2
+#define NUM_OCTAVE_LINE_DETECTOR 1
+
+typedef cv::Ptr<cv::FeatureDetector> FeatureDetectorPtr;
+typedef cv::Ptr<cv::line_descriptor::LSDDetector> LSDDetector;
+typedef cv::Ptr<cv::line_descriptor::BinaryDescriptor> EDDetector;
+
+/**
+ * \brief Feature detector parameters class
+ */
+class CDetector_Params: public CParams {
+public:
+	CDetector_Params(void) {
+	}
+	;
+	~CDetector_Params(void) {
+	}
+	;
+};
+
+/**
+ * \brief Main feature detector class
+ */
+class CFeature_Detector: public CVu_Base <CDetector_Params> {
+public:
+
+	/**
+	 * \brief Constructor
+	 */
+	CFeature_Detector(const std::string& _type, const CDetector_Params& _params);
+	CFeature_Detector(void);
+
+	/**
+	 * \brief Destructor
+	 */
+	~CFeature_Detector(void);
+
+	/**
+	 * \brief Inform if the detector searches points or lines
+	 */
+	bool isLine(void);
+
+	/**
+	 * \brief Inform if there is a maximum number of features to be detected
+	 */
+	bool isLimited(void);
+
+	/**
+	 * \brief Get number of maximum keypoints allowed
+	 */
+	int getKeyPointsLimit(void);
+
+	/**
+	 * \brief Set number of maximum keypoints allowed
+	 */
+	bool setLimitKeypts(unsigned int _nFeatures);
+
+	/**
+	 * \brief Detect features (points or lines)
+	 */
+	KeyPointVector detectKeyPoints(const cv::Mat& _image);
+	KeyPointVector detectKeyPoints(const cv::Mat& _image, const cv::Mat& _mask);
+	KeyLineVector detectKeyLines(const cv::Mat& _image);
+	KeyLineVector detectKeyLines(const cv::Mat& _image, const cv::Mat& _mask);
+
+private:
+
+	// Flags
+	bool is_line_;
+	bool is_limited_;
+
+	unsigned int keypoints_limit_; // Max. number of keypoints (saturation)
+
+	FeatureDetectorPtr feature_detector_; // Feature point detectors
+	LSDDetector lsd_detector_; // Feature line detector
+	EDDetector ed_detector_; // Feature line detector
+
+    /**
+     * \brief Set all types
+     */
+	void setAllTypes(void);
+
+	/**
+	 * \brief Initialize detector
+	 */
+	bool init(const std::string &_type, const CDetector_Params &_params);
+};
+
+#endif
diff --git a/src/feature_matcher/feature_matcher.cpp b/src/feature_matcher/feature_matcher.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de1b97a53bd2c30f3e811a6af26ec13564c0020e
--- /dev/null
+++ b/src/feature_matcher/feature_matcher.cpp
@@ -0,0 +1,195 @@
+#include "feature_matcher.h"
+
+CFeature_Matcher::CFeature_Matcher(const std::string& _type, const std::string& _match_out_type, const CMatcher_Params& _params)
+{
+    setAllTypes();
+
+    is_init_ = init(_type, _params);
+
+    if (!is_init_)
+    {
+        std::cerr << "[Feature Matcher]: Something went wrong during initialization! Feature Matcher not initialized."
+                << std::endl;
+    }
+}
+
+CFeature_Matcher::CFeature_Matcher(void)
+{
+    setAllTypes();
+}
+
+CFeature_Matcher::~CFeature_Matcher(void)
+{
+}
+
+void CFeature_Matcher::setAllTypes(void)
+{
+    // Define all types
+    std::vector<std::string> types;
+    types += "FlannBased", "BruteForce", "BruteForce-L1", "BruteForce-Hamming", "BruteForce-Hamming(2)";
+    types_.set(types);
+    type_ = "BruteForce"; // Default value
+
+    // Define all match output variants
+    std::vector<std::string> out_types;
+    out_types += "Match", "knnMatch", "radiusMatch";
+    match_search_types_.set(out_types);
+    match_search_type_ = "knnMatch"; // Default value
+}
+
+std::vector<std::string> CFeature_Matcher::listSearchTypes(void)
+{
+	return match_search_types_.list();
+}
+
+void CFeature_Matcher::getSearchType(int& _type)
+{
+	_type = match_search_types_(match_search_type_);
+}
+
+void CFeature_Matcher::getSearchType(std::string& _type)
+{
+	_type = match_search_type_;
+}
+
+bool CFeature_Matcher::setSearchType(const std::string& _type, const CMatcher_Params& _params)
+{
+    // TODO: Set parameters for each matcher type
+    for (unsigned int ii = 0; ii < match_search_types_.size(); ++ii)
+    {
+    	if (_type.compare(match_search_types_(ii))==0)
+    	{
+    	    	match_search_type_ = _type;
+    	    	match_search_params_ = _params;
+    	    	return true;
+    	}
+    }
+
+    std::cerr << "[Feature Matcher]: matcher output type " << _type << " doesn't exist !" << std::endl;
+    return false;
+}
+
+
+bool CFeature_Matcher::init(const std::string& _type, const CMatcher_Params& _params)
+{
+    if (is_init_)
+    {
+        std::cerr
+                << "[CFeature_Matcher::init]: Matcher already initialized."
+                << std::endl;
+        return false;
+    }
+
+    std::cout << _type << std::endl;
+
+
+    // TODO: Set parameters for each matcher type
+    if (_type.compare(types_(0))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(0);
+    }
+    else if (_type.compare(types_(1))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(1);
+    }
+    else if (_type.compare(types_(2))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(2);
+    }
+    else if (_type.compare(types_(3))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(3);
+    }
+    else if (_type.compare(types_(4))==0)
+    {
+        feature_matcher_ = cv::DescriptorMatcher::create(_type);
+        type_ = types_(4);
+    }
+    else
+    {
+        std::cerr << "[Feature Matcher]: mathcer_type " << _type << " doesn't exist !" << std::endl;
+        return false;
+    }
+
+    is_init_ = true;
+    return true;
+}
+
+// TODO: Match features and process matches (e.g. https://github.com/kipr/opencv/blob/master/samples/cpp/detector_descriptor_matcher_evaluation.cpp)
+void CFeature_Matcher::match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector<cv::DMatch>& matches)
+{
+	std::cout << __LINE__ << std::endl;
+    if (!is_init_)
+        std::cerr << "[CFeature_Matcher::match]: Matcher non initialized." << std::endl;
+
+    if (match_search_type_.compare(match_search_types_(0))==0)
+    {
+    	clock_t tStart = clock();
+    	// TODO: use parameters related with the search type (match_search_params_)
+    	std::cout << __LINE__ << " " << _desc1.size() << " " << _desc2.size() << std::endl;
+    	if (!_desc1.empty() && !_desc2.empty())
+    		feature_matcher_->match( _desc1, _desc2, matches);
+    	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+    }
+    else
+    	std::cerr << "[CFeature_Matcher::match]: The selected matcher output is different than your object." << std::endl;
+}
+
+// TODO: Match features and process matches (e.g. https://github.com/kipr/opencv/blob/master/samples/cpp/detector_descriptor_matcher_evaluation.cpp)
+void CFeature_Matcher::match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector< std::vector<cv::DMatch> >& matches)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Matcher::match]: Matcher non initialized." << std::endl;
+
+    if (match_search_type_.compare(match_search_types_(1))==0)// knn match
+    {
+    	clock_t tStart = clock();
+    	// TODO: use parameters related with the search type (match_search_params_)
+    	if (!_desc1.empty() && !_desc2.empty())
+    		feature_matcher_->knnMatch(_desc1, _desc2, matches, 2);
+    	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+    }
+    else if (match_search_type_.compare(match_search_types_(2))==0) // radius match
+	{
+    	clock_t tStart = clock();
+    	// TODO: use parameters related with the search type (match_search_params_)
+    	if (!_desc1.empty() && !_desc2.empty())
+    		feature_matcher_->radiusMatch(_desc1, _desc2, matches, 2);
+    	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+	}
+    else
+    	std::cerr << "[CFeature_Matcher::match]: The selected matcher output is different than your object." << std::endl;
+}
+
+void CFeature_Matcher::filterMatches(const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts)
+{
+	//only 25% of maximum of possible distance
+	double tresholdDist = 0.25 * sqrt(double(_img_height*_img_height + _img_width*_img_width));
+
+	std::vector< cv::DMatch > good_matches2;
+	_filtered_matches.reserve(_dirty.size());
+	for (size_t ii = 0; ii < _dirty.size(); ++ii)
+	{
+	    for (unsigned int jj = 0; jj < _dirty[ii].size(); jj++)
+	    {
+	        cv::Point2f from = _kpts1[_dirty[ii][jj].queryIdx].pt;
+	        cv::Point2f to = _kpts2[_dirty[ii][jj].trainIdx].pt;
+
+	        //calculate local distance for each possible match
+	        double dist = std::sqrt((from.x - to.x) * (from.x - to.x) + (from.y - to.y) * (from.y - to.y));
+
+	        //save as best match if local distance is in specified area and on same height
+	        if (dist < tresholdDist && std::abs(from.y-to.y)<5)
+	        {
+	        	_filtered_matches.push_back(_dirty[ii][jj]);
+	        	cv::KeyPoint kpt = cv::KeyPoint(to,1);
+	        	_filtered_kpts.push_back(kpt);
+	            jj = _dirty[ii].size();
+	        }
+	    }
+	}
+}
diff --git a/src/feature_matcher/feature_matcher.h b/src/feature_matcher/feature_matcher.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c5eda9a36f625f89a08bb430099ea8653f7d16b
--- /dev/null
+++ b/src/feature_matcher/feature_matcher.h
@@ -0,0 +1,81 @@
+#ifndef _FEATURE_MATCHER_H
+#define _FEATURE_MATCHER_H
+
+#include "../common/vu_base.h"
+
+typedef cv::Ptr<cv::DescriptorMatcher> FeatureMatcherPtr;
+
+/**
+ * \brief Feature descriptor parameters class
+ */
+class CMatcher_Params: public CParams {
+public:
+    CMatcher_Params(void) {
+    }
+    ;
+    ~CMatcher_Params(void) {
+    }
+    ;
+};
+
+class CFeature_Matcher: public CVu_Base<CMatcher_Params>
+{
+    public:
+
+        CFeature_Matcher(const std::string& _type, const std::string& _match_out_type, const CMatcher_Params& _params);
+        CFeature_Matcher(void);
+
+        ~CFeature_Matcher(void);
+
+        /**
+         * \brief list all possible match output types (All, K nearest, in a radius search, ...)
+         */
+        std::vector<std::string> listSearchTypes(void);
+
+        /**
+         * \brief Set matcher search method
+         */
+        bool setSearchType(const std::string& _type, const CMatcher_Params& _params);
+
+        /**
+         * \brief Get matcher search method
+         */
+        void getSearchType(int& _type);
+        void getSearchType(std::string& _type);
+        /**
+         * \brief Find Matches
+         */
+        void match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector<cv::DMatch>& matches);
+
+        /**
+         * \brief Find K best matches or matches in a radius (depending on selected match_search_type_)
+         */
+        void match(const cv::Mat& _desc1, const cv::Mat& _desc2, std::vector< std::vector<cv::DMatch> >& matches);
+
+        /**
+         * \brief 	Look whether the match is inside a defined area of the image
+         *
+         * 			From https://stackoverflow.com/questions/17967950/improve-matching-of-feature-points-with-opencv
+         */
+        void filterMatches(const KeyPointVector& _kpts1,const KeyPointVector& _kpts2, const std::vector< std::vector<cv::DMatch> >& _dirty, const int& _img_width, const int& _img_height, std::vector<cv::DMatch>& _filtered_matches, KeyPointVector& _filtered_kpts);
+
+    private:
+
+        FeatureMatcherPtr feature_matcher_; // Feature matcher
+
+        CTypes match_search_types_; // Match type (match with all, knn and radius search).
+        CMatcher_Params match_search_params_; // Search type parameters.
+        std::string match_search_type_; // Match type (match with all, knn and radius search).
+
+        /**
+         * \brief Set all types
+         */
+        void setAllTypes(void);
+
+        /**
+         * \brief Initialize matcher
+         */
+        bool init(const std::string &_type, const CMatcher_Params &_params);
+};
+
+#endif
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1776b3afacf6519ebf4ead7f93c4f31384bb6460
--- /dev/null
+++ b/src/vision_utils.cpp
@@ -0,0 +1,34 @@
+#include "vision_utils.h"
+
+CVision_Utils::CVision_Utils() {
+}
+
+CVision_Utils::~CVision_Utils() {
+}
+
+bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs)
+{
+    return (lhs.x < rhs.x) || ((lhs.x == rhs.x) && (lhs.y < rhs.y));
+}
+
+std::vector<cv::Point2f> vecIntersec(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
+{
+  std::vector<cv::Point2f> v3;
+  // Sort vectors
+  std::sort(v1.begin(), v1.end(), LessPoints);
+  std::sort(v2.begin(), v2.end(), LessPoints);
+  // Intersect
+  std::set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(v3), LessPoints);
+  return v3;
+}
+
+std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
+{
+  std::vector<cv::Point2f> v3;
+  // Sort vectors
+  std::sort(v1.begin(), v1.end(), LessPoints);
+  std::sort(v2.begin(), v2.end(), LessPoints);
+  // Intersect
+  std::set_union(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(v3), LessPoints);
+  return v3;
+}
diff --git a/src/vision_utils.h b/src/vision_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..a384bbbbaf163be055aed2afe542687aa878091b
--- /dev/null
+++ b/src/vision_utils.h
@@ -0,0 +1,38 @@
+#ifndef _VISION_UTILS_H
+#define _VISION_UTILS_H
+
+#include "cam_utils/cam_utils.h"
+#include "feature_detector/feature_detector.h"
+#include "feature_descriptor/feature_descriptor.h"
+#include "feature_matcher/feature_matcher.h"
+
+#include <functional>
+#include <set>
+
+class CVision_Utils {
+public:
+	CVision_Utils();
+	~CVision_Utils();
+};
+
+template <typename T>
+T readFromUser(const T& def_num)
+{
+	T read = def_num;
+	std::string input;
+	std::getline( std::cin, input );
+	if ( !input.empty() )
+	{
+	   std::istringstream stream( input );
+	   stream >> read;
+	}
+
+	return read;
+}
+
+bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs);
+std::vector<cv::Point2f> vecIntersec(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2);
+std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2);
+
+
+#endif