diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index c588c28637ad0e14480e5c2ed2309f04e8e12931..5c600911b1171a0f9b6c2e8b05326e3a3daa9ed9 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1476617952823822664" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1504749124108071272" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e29cb8d122877d06078a02ec65a39c8d511866ca..0f610a8a5bb23c5df57f99e562841a193cb9f33a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -7,6 +7,7 @@ SET(headers vision_utils.h vu_base/vu_base.h cam_utils/cam_utils.h feature_detec
 # 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)
@@ -23,13 +24,13 @@ endif(OpenCV_FOUND)
 
 
 # add the necessary include directories
-INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS})
+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}) 
+TARGET_LINK_LIBRARIES(vision_utils ${OpenCV_LIBS} ${Boost_LIBRARIES}) 
 
 # install 
 INSTALL(TARGETS vision_utils
diff --git a/src/examples/feature_detect.cpp b/src/examples/feature_detect.cpp
index b07669991a7c2d6abec5d6e333cb65d2a4c5616e..5f3dc8fbc8d113935303338e9b9e45c12fbb327f 100644
--- a/src/examples/feature_detect.cpp
+++ b/src/examples/feature_detect.cpp
@@ -50,14 +50,14 @@ int main(int argc, char *argv[])
 
         if (!detector.isLine())
         {
-            CFeature_Detector::KeyPointVector keypoints;
+            KeyPointVector keypoints;
             keypoints = detector.detectKeyPoints(frame);
             // Show frame with features
             detector.drawKeyFeatures(frame, keypoints);
         }
         else
         {
-            CFeature_Detector::KeyLineVector keypoints;
+            KeyLineVector keypoints;
             keypoints = detector.detectKeyLines(frame);
             // Show frame with features
             detector.drawKeyFeatures(frame, keypoints);
diff --git a/src/examples/feature_detect_descript.cpp b/src/examples/feature_detect_descript.cpp
index 1ff580f46f57cea8075dd77c84be0997db13485e..f6bea26e86c6b55ce730486275961c7be88ce2a9 100644
--- a/src/examples/feature_detect_descript.cpp
+++ b/src/examples/feature_detect_descript.cpp
@@ -67,7 +67,7 @@ int main(int argc, char *argv[])
         cv::imshow("Original image", frame);
 
       	// Detector
-        CFeature_Detector::KeyPointVector keypoints;
+        KeyPointVector keypoints;
         keypoints = detector.detectKeyPoints(frame);
 
         // Descriptor
diff --git a/src/examples/feature_detect_descript_match.cpp b/src/examples/feature_detect_descript_match.cpp
index dc076db4a3082b36c78dee7995c097809772b164..e4781450931b4cb3d8c2c885188caa969e48bd41 100644
--- a/src/examples/feature_detect_descript_match.cpp
+++ b/src/examples/feature_detect_descript_match.cpp
@@ -44,6 +44,7 @@ int main(int argc, char *argv[])
 
     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();
@@ -56,7 +57,21 @@ int main(int argc, char *argv[])
     std::cin >> match_type;
     matcher.set(matchers_list[match_type],matcher_params);
 
-    std::cout << std::endl << "Testing: " << detectors_list[feat_type] << " + " << descriptor_list[desc_type] << " + " << matchers_list[match_type] << " (DETECTOR + DESCRIPTOR + MATCHER)" << std::endl;
+    std::vector<std::string> matchers_out_list;
+    matchers_out_list = matcher.listSearchTypes();
+
+    for (unsigned int ii=0; ii<matchers_out_list.size(); ii++)
+        std::cout << "[" << ii << "]: " << matchers_out_list[ii] << std::endl;
+
+    std::cout << std::endl << "Which MATCHER SEARCH do you want to test?: ";
+    int match_search_type;
+    std::cin >> match_search_type;
+    matcher.setSearchType(matchers_out_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_out_list[match_search_type] << " (DETECTOR + DESCRIPTOR + MATCHER + MATCHER SEARCH)" << std::endl;
+
+    // matcher objects
+    KeyPointVector keypoints_old;
 
     // Open camera
     cv::VideoCapture cam;
@@ -81,19 +96,20 @@ int main(int argc, char *argv[])
         cv::imshow("Original image", frame);
 
       	// Detector
-        CFeature_Detector::KeyPointVector keypoints;
+        KeyPointVector keypoints;
         keypoints = detector.detectKeyPoints(frame);
 
         // Descriptor
         cv::Mat descriptors;
         descriptors = descriptor.getDescriptor(frame,keypoints);
 
-        // TODO: Implement matcher
+//        // Matcher
 //        if  (nframe > 0)
 //        {
-//            cv::Mat matchs;
-//            matchs = matcher.match();
+//        	std::vector<cv::DMatch> matches;
+//        	matcher.match(keypoints,keypoints_old,matches);
 //        }
+//        keypoints_old = keypoints;
 
         // Show frame with features
         detector.drawKeyFeatures(frame, keypoints);
diff --git a/src/feature_descriptor/feature_descriptor.cpp b/src/feature_descriptor/feature_descriptor.cpp
index 552bd02be6ed70c34fdd8ba4c957a98cb2e38970..bb11a416d8b755671cba3dadde74e92f3157f1ca 100644
--- a/src/feature_descriptor/feature_descriptor.cpp
+++ b/src/feature_descriptor/feature_descriptor.cpp
@@ -104,7 +104,7 @@ bool CFeature_Descriptor::init(const std::string& _type, const CDescriptor_Param
     return true;
 }
 
-cv::Mat CFeature_Descriptor::getDescriptor(const cv::Mat& _image, CFeature_Descriptor::KeyPointVector& _kpts)
+cv::Mat CFeature_Descriptor::getDescriptor(const cv::Mat& _image, KeyPointVector& _kpts)
 {
     if (!is_init_)
         std::cerr << "[CFeature_Descriptor::getDescriptor]: Descriptor non initialized." << std::endl;
diff --git a/src/feature_descriptor/feature_descriptor.h b/src/feature_descriptor/feature_descriptor.h
index 829fba91f29bc90f586f4dab377b5d054c816066..9020f6bd2c8f68eecf0acd6dce378cc5e9ac60d3 100644
--- a/src/feature_descriptor/feature_descriptor.h
+++ b/src/feature_descriptor/feature_descriptor.h
@@ -1,21 +1,8 @@
 #ifndef _FEATURE_DESCRIPTOR_H
 #define _FEATURE_DESCRIPTOR_H
 
-#include <time.h>
-
-// Own
 #include <vu_base/vu_base.h>
 
-// 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 cv::Ptr<cv::DescriptorExtractor> FeatureDescriptorPtr;
 
 /**
@@ -35,8 +22,6 @@ class CFeature_Descriptor: public CVu_Base <CDescriptor_Params>{
 
 public:
 
-	typedef std::vector<cv::KeyPoint> KeyPointVector;
-
 	CFeature_Descriptor(const std::string& _type, const CDescriptor_Params& _params);
 	CFeature_Descriptor(void);
 
@@ -46,7 +31,7 @@ public:
 	 * \brief Get descriptors
 	 */
 	cv::Mat getDescriptor(const cv::Mat& _image,
-			CFeature_Descriptor::KeyPointVector& _kpts);
+			KeyPointVector& _kpts);
 
 private:
 
diff --git a/src/feature_detector/feature_detector.cpp b/src/feature_detector/feature_detector.cpp
index 4c091bc245b9ba72ca5ff0ef6ee01bb826ec5d6d..be6ee85469f3b229a184ce8178fb651c735a3cba 100644
--- a/src/feature_detector/feature_detector.cpp
+++ b/src/feature_detector/feature_detector.cpp
@@ -136,13 +136,13 @@ int CFeature_Detector::getKeyPointsLimit(void)
     return keypoints_limit_;
 }
 
-CFeature_Detector::KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image)
+KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image)
 {
 	cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
 	return detectKeyPoints(_image, mask);
 }
 
-CFeature_Detector::KeyPointVector CFeature_Detector::detectKeyPoints(const cv::Mat& _image, const cv::Mat& _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;
@@ -162,13 +162,13 @@ CFeature_Detector::KeyPointVector CFeature_Detector::detectKeyPoints(const cv::M
     return kpts;
 }
 
-CFeature_Detector::KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image)
+KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image)
 {
 	cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
 	return detectKeyLines(_image, mask);
 }
 
-CFeature_Detector::KeyLineVector CFeature_Detector::detectKeyLines(const cv::Mat& _image, const cv::Mat& _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;
@@ -198,24 +198,4 @@ bool CFeature_Detector::setLimitKeypts(unsigned int _nFeatures)
     return false;
 }
 
-cv::Mat CFeature_Detector::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 CFeature_Detector::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;
-}
-
 
diff --git a/src/feature_detector/feature_detector.h b/src/feature_detector/feature_detector.h
index ae04e8506ab3c407d72f2d3f5695517f22521b83..4d1bf2055752ba78a204fb47adf9cb828237bbed 100644
--- a/src/feature_detector/feature_detector.h
+++ b/src/feature_detector/feature_detector.h
@@ -1,20 +1,8 @@
 #ifndef _FEATURE_DETECTOR_H
 #define _FEATURE_DETECTOR_H
 
-#include <time.h>
-
-// Own
 #include <vu_base/vu_base.h>
 
-// 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/line_descriptor/descriptor.hpp>
-
 #define SCALE_FACTOR_LINE_DETECTOR 2
 #define NUM_OCTAVE_LINE_DETECTOR 1
 
@@ -41,9 +29,6 @@ public:
 class CFeature_Detector: public CVu_Base <CDetector_Params> {
 public:
 
-	typedef std::vector<cv::KeyPoint> KeyPointVector;
-	typedef std::vector<cv::line_descriptor::KeyLine> KeyLineVector;
-
 	/**
 	 * \brief Constructor
 	 */
@@ -83,14 +68,6 @@ public:
 	KeyLineVector detectKeyLines(const cv::Mat& _image);
 	KeyLineVector detectKeyLines(const cv::Mat& _image, const cv::Mat& _mask);
 
-	/**
-	 * \brief Draw detected features
-	 */
-	cv::Mat drawKeyFeatures(const cv::Mat& _image,
-			const KeyPointVector& _kp_vec);
-	cv::Mat drawKeyFeatures(const cv::Mat& _image,
-			const KeyLineVector& _kl_vec);
-
 private:
 
 	// Flags
diff --git a/src/feature_matcher/feature_matcher.cpp b/src/feature_matcher/feature_matcher.cpp
index af8bf18aebcae5083b08c9a1c56d942b57afbfef..34ff0b3722fc0ae7b645a745b1220539f6efb1a5 100644
--- a/src/feature_matcher/feature_matcher.cpp
+++ b/src/feature_matcher/feature_matcher.cpp
@@ -1,6 +1,6 @@
 #include "feature_matcher.h"
 
-CFeature_Matcher::CFeature_Matcher(const std::string& _type, const CMatcher_Params& _params)
+CFeature_Matcher::CFeature_Matcher(const std::string& _type, const std::string& _match_out_type, const CMatcher_Params& _params)
 {
     setAllTypes();
 
@@ -28,6 +28,35 @@ void CFeature_Matcher::setAllTypes(void)
     std::vector<std::string> types;
     types += "FlannBased", "BruteForce", "BruteForce-L1", "BruteForce-Hamming", "BruteForce-Hamming(2)";
     types_.set(types);
+
+    // Define all match output variants
+    std::vector<std::string> out_types;
+    out_types += "Match", "knnMartch", "radiusMatch";
+    match_search_types_.set(out_types);
+
+    match_search_type_ = "Match"; // Default value
+}
+
+std::vector<std::string> CFeature_Matcher::listSearchTypes(void)
+{
+	return match_search_types_.list();
+}
+
+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;
 }
 
 
@@ -80,22 +109,44 @@ bool CFeature_Matcher::init(const std::string& _type, const CMatcher_Params& _pa
     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 KeyPointVector& _kpts1, const KeyPointVector& _kpts2, std::vector<cv::DMatch>& matches)
+{
+    if (!is_init_)
+        std::cerr << "[CFeature_Matcher::match]: Matcher non initialized." << std::endl;
 
-//cv::Mat CFeature_Matcher::match(const cv::Mat& _image, CFeature_Descriptor::KeyPointVector& _kpts)
-//{
-//    if (!is_init_)
-//        std::cerr << "[CFeature_Matcher::match]: Matcher non initialized." << std::endl;
-//
-//    cv::Mat descriptors;
-//
-//    clock_t tStart = clock();
-//    feature_matcher_->compute(_image, _kpts, descriptors);
-//    comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
-//
-//    return descriptors;
-//}
-
+    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_)
+    	feature_matcher_->match( _kpts1, _kpts2, 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 KeyPointVector& _kpts1, const KeyPointVector& _kpts2, 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_)
+    	feature_matcher_->knnMatch(_kpts1, _kpts2, 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_)
+    	feature_matcher_->radiusMatch(_kpts1, _kpts2, 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;
+}
diff --git a/src/feature_matcher/feature_matcher.h b/src/feature_matcher/feature_matcher.h
index e13f44cde116fc03515b94d0cda22ddf9bc85aaa..9cc8ebfd18eb0a9f78a5c402ae72631619d4fd0d 100644
--- a/src/feature_matcher/feature_matcher.h
+++ b/src/feature_matcher/feature_matcher.h
@@ -1,21 +1,8 @@
 #ifndef _FEATURE_MATCHER_H
 #define _FEATURE_MATCHER_H
 
-#include <time.h>
-
-// own
 #include "vu_base/vu_base.h"
 
-// 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 cv::Ptr<cv::DescriptorMatcher> FeatureMatcherPtr;
 
 /**
@@ -35,21 +22,39 @@ class CFeature_Matcher: public CVu_Base<CMatcher_Params>
 {
     public:
 
-        CFeature_Matcher(const std::string& _type, const CMatcher_Params& _params);
+        CFeature_Matcher(const std::string& _type, const std::string& _match_out_type, const CMatcher_Params& _params);
         CFeature_Matcher(void);
 
         ~CFeature_Matcher(void);
 
         /**
-         * \brief Match
+         * \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 Find Matches
+         */
+        void match(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, std::vector<cv::DMatch>& matches);
+
+        /**
+         * \brief Find K best matches or matches in a radius (depending on selected match_search_type_)
          */
-//        cv::Mat match(const cv::Mat& _image,
-//                CFeature_Descriptor::KeyPointVector& _kpts);
+        void match(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, std::vector< std::vector<cv::DMatch> >& matches);
 
     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
          */
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
index 26e6254853aa5de9f4bc6d2a3cd695c5ad49f4c3..922bbd6aa97ef6bbf7b82b039d96b14cc324622d 100644
--- a/src/vision_utils.cpp
+++ b/src/vision_utils.cpp
@@ -5,4 +5,3 @@ CVision_Utils::CVision_Utils() {
 
 CVision_Utils::~CVision_Utils() {
 }
-
diff --git a/src/vu_base/vu_base.h b/src/vu_base/vu_base.h
index 9c1af28563c26c17bcba9100dffb5b08a8498535..e6e57abc20abffd44a386d26f0dbd0bf6f920f73 100644
--- a/src/vu_base/vu_base.h
+++ b/src/vu_base/vu_base.h
@@ -1,18 +1,34 @@
 #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
  */
@@ -136,6 +152,27 @@ public:
 	    return success;
 	};
 
+	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