diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 480d47aaef918319f0882fccf493770937b87289..3a68cd19a9be673118d2deaaad0005efea56b541 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Specific definitions
-ADD_DEFINITIONS(-DPRINT_INFO_VU)
+#ADD_DEFINITIONS(-DPRINT_INFO_VU)
 
 # library source files
 SET(sources
diff --git a/src/algorithms/activesearch/alg_activesearch.h b/src/algorithms/activesearch/alg_activesearch.h
index 7a4007fd7e9f44d6e299dbbfbd40e161a2a954b0..e4949d2c3370f2cf93e9bab833e1c9e0506e1d94 100644
--- a/src/algorithms/activesearch/alg_activesearch.h
+++ b/src/algorithms/activesearch/alg_activesearch.h
@@ -1,6 +1,7 @@
 #ifndef _ALGORITHM_ACTIVESEARCH_H_
 #define _ALGORITHM_ACTIVESEARCH_H_
 
+#include "../../vision_utils.h"
 #include "../algorithm_base.h"
 #include "../algorithm_factory.h"
 #include "../../common_class/buffer.h"
@@ -9,15 +10,6 @@
 #include "../../descriptors/descriptor_base.h"
 #include "../../matchers/matcher_base.h"
 
-//OpenCV
-#include <opencv2/core.hpp>
-#include <opencv2/features2d.hpp>
-
-// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-#include <eigen3/Eigen/Sparse>
-
 namespace vision_utils {
 
 // Create all pointers
diff --git a/src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h b/src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h
index f6831ce5043f7ef47fcd0d7abf1bc830bfca0094..e18c766694ce3d4f33193fc7054e5b2289c80584 100644
--- a/src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h
+++ b/src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h
@@ -1,6 +1,7 @@
 #ifndef _ALGORITHM_OPTFLOWPYRLK_H_
 #define _ALGORITHM_OPTFLOWPYRLK_H_
 
+#include "../../vision_utils.h"
 #include "../algorithm_base.h"
 #include "../algorithm_factory.h"
 #include "../../common_class/buffer.h"
diff --git a/src/algorithms/trackfeatures/alg_trackfeatures.h b/src/algorithms/trackfeatures/alg_trackfeatures.h
index 32209cc5d8eb26d515e654782869b0822c8e6691..38cc8a18997d42d07e31c7ba799f52a714cf7c12 100644
--- a/src/algorithms/trackfeatures/alg_trackfeatures.h
+++ b/src/algorithms/trackfeatures/alg_trackfeatures.h
@@ -1,6 +1,7 @@
 #ifndef _ALGORITHM_TRACKFEATURES_H_
 #define _ALGORITHM_TRACKFEATURES_H_
 
+#include "../../vision_utils.h"
 #include "../algorithm_base.h"
 #include "../algorithm_factory.h"
 #include "../../common_class/buffer.h"
@@ -9,15 +10,6 @@
 #include "../../descriptors/descriptor_base.h"
 #include "../../matchers/matcher_base.h"
 
-//OpenCV
-#include <opencv2/core.hpp>
-#include "opencv2/features2d.hpp"
-
-// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-#include <eigen3/Eigen/Sparse>
-
 namespace vision_utils {
 
 // Create all pointers
diff --git a/src/detectors/detector_base.cpp b/src/detectors/detector_base.cpp
index 43c8ad54f77f626812735790e8d3109ffa256376..559612d760df7058d1d78475722df1e17ec682ce 100644
--- a/src/detectors/detector_base.cpp
+++ b/src/detectors/detector_base.cpp
@@ -10,25 +10,17 @@ DetectorBase::~DetectorBase(void)
 {
 }
 
-KeyPointVector DetectorBase::detect(const cv::Mat& _image)
-{
-    cv::Mat mask = cv::Mat::ones(_image.size(), CV_8U);
-	return detect(_image, mask);
-}
-
-KeyPointVector DetectorBase::detect(const cv::Mat& _image, const cv::Mat& _mask)
+KeyPointVector DetectorBase::detect(const cv::Mat& _image, const cv::InputArray& _mask)
 {
 	KeyPointVector kpts;
 	clock_t tStart = clock();
 	detector_->detect(_image, kpts, _mask);
 	comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
-
 	return kpts;
 }
 
 KeyPointVector DetectorBase::detect(const cv::Mat& _image, cv::Rect& _roi)
 {
-	// FIX this
 	KeyPointVector kpts;
 	if (!_image.empty())
 	{
diff --git a/src/detectors/detector_base.h b/src/detectors/detector_base.h
index b5ae64d5ea3a64a19ac9e50fd9959b1164b13380..dc02b168330da646d8007a8e3d4cc152ba8b8918 100644
--- a/src/detectors/detector_base.h
+++ b/src/detectors/detector_base.h
@@ -34,8 +34,7 @@ class DetectorBase : public VUBase, public std::enable_shared_from_this<Detector
          */
         virtual ~DetectorBase(void);
 
-        KeyPointVector detect(const cv::Mat& _image);
-        KeyPointVector detect(const cv::Mat& _image, const cv::Mat& _mask);
+        KeyPointVector detect(const cv::Mat& _image, const cv::InputArray& _mask=cv::noArray() );
         KeyPointVector detect(const cv::Mat& _image, cv::Rect& _roi);
 
         std::string getName(void);
diff --git a/src/detectors/orb/detector_orb.h b/src/detectors/orb/detector_orb.h
index f7431a4b6931e2368878ff6631751baf7f574236..0a56f682470ec1b58566244750090bb080d42435 100644
--- a/src/detectors/orb/detector_orb.h
+++ b/src/detectors/orb/detector_orb.h
@@ -15,10 +15,10 @@ VU_PTR_TYPEDEFS(DetectorParamsORB);
  */
 struct DetectorParamsORB: public ParamsBase
 {
-	int nfeatures = 500;					// The maximum number of features to retain.
-	float scaleFactor = 2.0f;  				// Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.
+	int nfeatures = 20;					// The maximum number of features to retain.
+	float scaleFactor = 1.2f;  				// Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.
 	int nlevels = 8;						// The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).
-	int edgeThreshold = 31;					// This is size of the border where the features are not detected. It should roughly match the patchSize parameter.
+	int edgeThreshold = 16;					// This is size of the border where the features are not detected. It should roughly match the patchSize parameter.
 	int firstLevel = 0;						// It should be 0 in the current implementation.
 	int WTA_K = 2;							// The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).
 	int scoreType = cv::ORB::HARRIS_SCORE;	// The default HARRIS_SCORE means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
index 877cd3d4939f3075249c3a253a21e8d7d85f31b7..7ce4c3d3d873fa8337a9ae99b78e1ddaa66bc954 100644
--- a/src/vision_utils.cpp
+++ b/src/vision_utils.cpp
@@ -227,6 +227,39 @@ void retainBest(KeyPointVector& _kpts, int n)
     }
 }
 
+Eigen::MatrixXf projectPoints(const Eigen::MatrixXf& _points3D, const Eigen::Vector3f& _rot_mat, const Eigen::Vector3f& _trans_mat, const Eigen::MatrixXf& _cam_mat, const Eigen::VectorXf& _dist_coef)
+{
+	// Check row-wise points
+	assert(_points3D.cols()!=3 && "[Vision Utils]: The 3D points are expected to be stored row-wise");
+
+	cv::Mat cv_3Dpoints;
+	cv::eigen2cv(_points3D, cv_3Dpoints);
+
+	cv::Vec3f cv_rot_mat;
+	cv::eigen2cv(_rot_mat, cv_rot_mat);
+
+	cv::Vec3f cv_trans_mat;
+	cv::eigen2cv(_trans_mat, cv_trans_mat);
+
+	cv::Mat cv_cam_mat;
+	cv::eigen2cv(_cam_mat, cv_cam_mat);
+
+	cv::Mat cv_distcoeff_mat;
+	cv::eigen2cv(_dist_coef, cv_distcoeff_mat);
+
+	std::vector<cv::Point2f> cv_2Dpoints;
+
+	cv::projectPoints(cv_3Dpoints,cv_rot_mat,cv_trans_mat,cv_cam_mat,cv_distcoeff_mat,cv_2Dpoints);
+
+	Eigen::MatrixXf _points2D(cv_2Dpoints.size(),2);
+	for (int ii = 0; ii < cv_2Dpoints.size(); ++ii)
+	{
+		_points2D(ii,0) = cv_2Dpoints[ii].x;
+		_points2D(ii,1) = cv_2Dpoints[ii].y;
+	}
+	return _points2D;
+}
+
 #ifdef USING_YAML
 std::string readYamlType(const std::string& _filename_dot_yaml, const std::string& _type)
 {
diff --git a/src/vision_utils.h b/src/vision_utils.h
index 16db9354bc9bb812c638a39d7c41946a1ebdc419..c47aeb0f2f30a5e82f2de592bb1f67f8faae6bbf 100644
--- a/src/vision_utils.h
+++ b/src/vision_utils.h
@@ -18,9 +18,15 @@
 #include <boost/assign/std/vector.hpp> // for 'operator+=()'
 using namespace boost::assign; // bring 'operator+=()' into scope
 
+// Eigen
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
 // OpenCV
 #include <opencv2/core/core.hpp>
 #include <opencv2/core/types.hpp>
+#include <opencv2/core/eigen.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -180,6 +186,8 @@ std::string readYamlType(const std::string& _filename_dot_yaml, const std::strin
 
 void retainBest(KeyPointVector& _kpts, int n);
 
+Eigen::MatrixXf projectPoints(const Eigen::MatrixXf& _points3D, const Eigen::Vector3f& _rot_mat, const Eigen::Vector3f& _trans_mat, const Eigen::MatrixXf& _cam_mat, const Eigen::VectorXf& _dist_coef);
+
 } /* namespace vision_utils */
 
 #endif