diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 9bc4cc8d46dc83971dbc10ab2cea8243713c2fc2..494a9a5ccbca3a3ea5f6571cf3bafbeccde118a3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,3 +1,6 @@
+# Specific definitions
+ADD_DEFINITIONS(-DPRINT_INFO_VU)
+
 # library source files
 SET(sources 
 	vision_utils.cpp 
diff --git a/src/algorithms/algorithm_factory.h b/src/algorithms/algorithm_factory.h
index b05a50ab1a1ca084f193970115bbc45500698b04..c626bfd4d919b58e76cf216897e50209f451628e 100644
--- a/src/algorithms/algorithm_factory.h
+++ b/src/algorithms/algorithm_factory.h
@@ -20,7 +20,7 @@ typedef Factory<AlgorithmBase,
 template<>
 inline std::string AlgorithmFactory::getClass()
 {
-    return "AlgorithmFactory";
+    return "Algorithms Factory";
 }
 
 #define VU_REGISTER_ALGORITHM(AlgorithmType, AlgorithmName) \
diff --git a/src/descriptors/descriptor_factory.h b/src/descriptors/descriptor_factory.h
index 0ce949336853686a143869ff25910e2839146591..7ecec9c8588d3ea14b57fc96f608870bdf0ce6ef 100644
--- a/src/descriptors/descriptor_factory.h
+++ b/src/descriptors/descriptor_factory.h
@@ -20,7 +20,7 @@ typedef Factory<DescriptorBase,
 template<>
 inline std::string DescriptorFactory::getClass()
 {
-    return "DescriptorFactory";
+    return "Descriptors Factory";
 }
 
 #define VU_REGISTER_DESCRIPTOR(DescriptorType, DescriptorName) \
diff --git a/src/detectors/detector_base.cpp b/src/detectors/detector_base.cpp
index 2b6e87eac33c50bb66b0384d94d622866eb0a916..661e2c91ab0f15383cbaba4bad72f11ca7e004c6 100644
--- a/src/detectors/detector_base.cpp
+++ b/src/detectors/detector_base.cpp
@@ -25,6 +25,23 @@ KeyPointVector DetectorBase::detect(const cv::Mat& _image, const cv::Mat& _mask)
 	return kpts;
 }
 
+
+KeyPointVector DetectorBase::detect(cv::Mat& _image, cv::Rect& _roi, const int& _kp_inflation_rate)
+{
+    cv::Mat _image_roi;
+    adaptRoi(_kp_inflation_rate, _image_roi, _image, _roi);
+
+    KeyPointVector kpts;
+    kpts = detect(_image_roi);
+
+    for (auto kp : kpts)
+    {
+    	kp.pt.x = kp.pt.x + _roi.x;
+    	kp.pt.y = kp.pt.y + _roi.y;
+    }
+    return kpts;
+}
+
 } /* namespace vision_utils */
 
 #include "detector_factory.h"
diff --git a/src/detectors/detector_base.h b/src/detectors/detector_base.h
index 8671f846167d4339e700b3003ddc02e2aa3660fa..90e99a6f20210c86922a0ce9cc44b85ee2aefbc6 100644
--- a/src/detectors/detector_base.h
+++ b/src/detectors/detector_base.h
@@ -36,6 +36,7 @@ class DetectorBase : public VUBase, public std::enable_shared_from_this<Detector
 
         KeyPointVector detect(const cv::Mat& _image);
         KeyPointVector detect(const cv::Mat& _image, const cv::Mat& _mask);
+        KeyPointVector detect(cv::Mat& _image, cv::Rect& _roi, const int& _kp_inflation_rate);
 
         std::string getName(void);
 
diff --git a/src/detectors/detector_factory.h b/src/detectors/detector_factory.h
index 6bba993895df2e85ef761f130317a6cd5b1807f0..6073115e2223760c6ce4fac2a17f58873a2c89c4 100644
--- a/src/detectors/detector_factory.h
+++ b/src/detectors/detector_factory.h
@@ -20,7 +20,7 @@ typedef Factory<DetectorBase,
 template<>
 inline std::string DetectorFactory::getClass()
 {
-    return "DetectorFactory";
+    return "Detectors Factory";
 }
 
 #define VU_REGISTER_DETECTOR(DetectorType, DetectorName) \
diff --git a/src/examples/test_algorithm_opticalflowpyrlk.cpp b/src/examples/test_algorithm_opticalflowpyrlk.cpp
index 0aec025b175d88e039aee66ae222e1aa8bfa8a99..e4b659fdb8875b673af2d4b050c58658aab88718 100644
--- a/src/examples/test_algorithm_opticalflowpyrlk.cpp
+++ b/src/examples/test_algorithm_opticalflowpyrlk.cpp
@@ -24,7 +24,7 @@
 // Algorithms
 #include "../algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h"
 
-#define MIN_GOOD_POINTS 10
+#define MIN_GOOD_POINTS 20
 
 int main(void)
 {
@@ -78,7 +78,7 @@ int main(void)
 
     std::string def_alg = "OPTFLOWPYRLK";
 
-    AlgorithmBasePtr alg_ptr = setupAlgorithm(def_alg, def_alg + " matcher", vu_root + path_yaml_file + "/" + def_alg + ".yaml");
+    AlgorithmBasePtr alg_ptr = setupAlgorithm(def_alg, def_alg + " algorithm", vu_root + path_yaml_file + "/" + def_alg + ".yaml");
 
     AlgorithmOPTFLOWPYRLKPtr alg_of_ptr = std::static_pointer_cast<AlgorithmOPTFLOWPYRLK>(alg_ptr);
 
diff --git a/src/factory.h b/src/factory.h
index f9500ed19a24ef65aecf41e390174242338991c2..72280b68836b80edbc2529002d821b9df51102cf 100644
--- a/src/factory.h
+++ b/src/factory.h
@@ -40,19 +40,31 @@ class Factory
     private:
         Factory()  { }
         ~Factory() { }
+        bool init_ = false;
 };
 
 template<class TypeBase, typename... TypeInput>
 inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn, const bool& print)
 {
     bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
-    if (print)
-    {
-    	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;
-    }
+
+    #if defined (PRINT_INFO_VU)
+    	if (!init_)
+    	{
+    		if (!getClass().empty())
+    			std::cout << "==== " << getClass() << " ====" << std::endl;
+    		init_ = true;
+    	}
+
+    	if (print)
+    	{
+    		if (reg)
+    			std::cout << std::setw(2) << std::left << "->" << _type << std::endl;
+    		else
+    			std::cout << std::setw(2) << std::left << "-x skipping  " << _type << ": already registered." << std::endl;
+    	}
+	#endif
+
     return reg;
 }
 
@@ -95,7 +107,7 @@ typedef Factory<ParamsBase,
 template<>
 inline std::string ParamsFactory::getClass()
 {
-    return "ParamsFactory";
+    return "Params Factory";
 }
 
 } /* namespace vision_utils */
diff --git a/src/matchers/matcher_factory.h b/src/matchers/matcher_factory.h
index b1f75692c6bf9c07d73469468297dea1aa415289..0f9a126c1a4058431b44fa7ebebd04bfb0e53306 100644
--- a/src/matchers/matcher_factory.h
+++ b/src/matchers/matcher_factory.h
@@ -20,7 +20,7 @@ typedef Factory<MatcherBase,
 template<>
 inline std::string MatcherFactory::getClass()
 {
-    return "MatcherFactory";
+    return "Matchers Factory";
 }
 
 #define VU_REGISTER_MATCHER(MatcherType, MatcherName) \
diff --git a/src/sensors/sensor_factory.h b/src/sensors/sensor_factory.h
index d3567245734b3814276ca3b295d3de498d9d43bf..deef102dbd14985cddd809fadbf3793ae4c29ce1 100644
--- a/src/sensors/sensor_factory.h
+++ b/src/sensors/sensor_factory.h
@@ -20,7 +20,7 @@ typedef Factory<SensorBase,
 template<>
 inline std::string SensorFactory::getClass()
 {
-    return "SensorFactory";
+    return "Sensors Factory";
 }
 
 #define VU_REGISTER_SENSOR(SensorType, SensorName) \
diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp
index a5193af72a4a330db63ac87a7545d4ee080a4fb6..020679a980ffa0678a35954d32a08658d4459389 100644
--- a/src/vision_utils.cpp
+++ b/src/vision_utils.cpp
@@ -3,6 +3,52 @@
 namespace vision_utils
 {
 
+bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs)
+{
+    return (lhs.x < rhs.x) || ((lhs.x == rhs.x) && (lhs.y < rhs.y));
+};
+
+bool LessKPoints(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs)
+{
+    return (lhs.pt.x < rhs.pt.x) || ((lhs.pt.x == rhs.pt.x) && (lhs.pt.y < rhs.pt.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)
+{
+	// Sort vectors and intersect
+  std::vector<cv::Point2f> v3;
+  std::sort(v1.begin(), v1.end(), LessPoints);
+  std::sort(v2.begin(), v2.end(), LessPoints);
+  std::set_union(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(v3), LessPoints);
+  return v3;
+};
+
+void copyKPVector(const KeyPointVector& _kpv_from, KeyPointVector& _kpv_to)
+{
+	_kpv_to.clear();
+	for (auto kp : _kpv_from)
+		_kpv_to.push_back(kp);
+};
+
+PointVector KPToP(const KeyPointVector& _kp_vec)
+{
+    PointVector pt_vec;
+    for (auto kp : _kp_vec)
+    	pt_vec.push_back(kp.pt);
+    return pt_vec;
+};
+
 void whoHasMoved(const KeyPointVector& _kp_vec_1, const KeyPointVector& _kp_vec_2, KeyPointVector& _common_kp_vec, KeyPointVector& _exclusive_in_kp_vec_2)
 {
     _exclusive_in_kp_vec_2.clear();
@@ -51,5 +97,66 @@ void whoHasMoved(const PointVector& _pt_vec_1, const PointVector& _pt_vec_2, Poi
     }
 }
 
+void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius, const cv::Scalar& _color, const int& _thickness)
+{
+    for (auto kp : _kp_vec)
+        cv::circle(_image, kp.pt, _radius, _color, _thickness);
+};
+
+inline void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color)
+{
+    for (auto kl : _kl_vec)
+        cv::line(_image, kl.getStartPoint(), kl.getEndPoint(), _color, 3);
+};
+
+inline void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const cv::Scalar& _color)
+{
+    for (auto roi : _roi_vec)
+        cv::rectangle(_image, roi, _color, 1, 8, 0);
+
+    cv::imshow("Feature tracker", _image);
+}
+
+inline void trimRoi(const int& _img_width, const int& _img_height, cv::Rect& _roi)
+{
+    if(_roi.x < 0)
+    {
+        int diff_x = -_roi.x;
+        _roi.x = 0;
+        _roi.width = _roi.width - diff_x;
+    }
+    if(_roi.y < 0)
+    {
+        int diff_y = -_roi.y;
+        _roi.y = 0;
+        _roi.height = _roi.height - diff_y;
+    }
+    if((unsigned int)(_roi.x + _roi.width) > _img_width)
+    {
+        int diff_width = _img_width - (_roi.x + _roi.width);
+        _roi.width = _roi.width+diff_width;
+    }
+    if((unsigned int)(_roi.y + _roi.height) > _img_height)
+    {
+        int diff_height = _img_height - (_roi.y + _roi.height);
+        _roi.height = _roi.height+diff_height;
+    }
+}
+
+void inflateRoi(const int& _inflation_rate, cv::Rect& _roi)
+{
+    _roi.x = _roi.x - _inflation_rate;
+    _roi.y = _roi.y - _inflation_rate;
+    _roi.width = _roi.width + 2*_inflation_rate;
+    _roi.height = _roi.height + 2*_inflation_rate;
+}
+
+void adaptRoi(const int& _inflation_rate, cv::Mat& _image_roi, cv::Mat _image, cv::Rect& _roi)
+{
+    inflateRoi(_inflation_rate, _roi);
+    trimRoi(_image.size().width, _image.size().height, _roi);
+    _image_roi = _image(_roi);
+}
+
 } /* namespace vision_utils */
 
diff --git a/src/vision_utils.h b/src/vision_utils.h
index c60bc76a32dd7c141b8c3fff51f2e03273652248..e4b22ad8da1a3a7bdab10bcb5d1e1be35395a347 100644
--- a/src/vision_utils.h
+++ b/src/vision_utils.h
@@ -126,74 +126,32 @@ T readFromUser(const T& _default)
 	return read;
 };
 
-inline bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs)
-{
-    return (lhs.x < rhs.x) || ((lhs.x == rhs.x) && (lhs.y < rhs.y));
-};
+bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs);
 
-inline bool LessKPoints(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs)
-{
-    return (lhs.pt.x < rhs.pt.x) || ((lhs.pt.x == rhs.pt.x) && (lhs.pt.y < rhs.pt.y));
-};
+bool LessKPoints(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs);
 
-inline 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> vecIntersec(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2);
 
-inline std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
-{
-	// Sort vectors and intersect
-  std::vector<cv::Point2f> v3;
-  std::sort(v1.begin(), v1.end(), LessPoints);
-  std::sort(v2.begin(), v2.end(), LessPoints);
-  std::set_union(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);
 
-inline void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius=5, const cv::Scalar& _color = cv::Scalar(0, 0, 255), const int& _thickness=-1)
-{
-    for (auto kp : _kp_vec)
-        cv::circle(_image, kp.pt, _radius, _color, _thickness);
-};
+void copyKPVector(const KeyPointVector& _kpv_from, KeyPointVector& _kpv_to);
 
-inline void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255) )
-{
-    for (auto kl : _kl_vec)
-        cv::line(_image, kl.getStartPoint(), kl.getEndPoint(), _color, 3);
-};
+PointVector KPToP(const KeyPointVector& _kp_vec);
 
-inline void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255))
-{
-    for (auto roi : _roi_vec)
-        cv::rectangle(_image, roi, _color, 1, 8, 0);
+void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2);
+void whoHasMoved(const PointVector& _pts1, const PointVector& _pts2, PointVector& _common_pts, PointVector& _new_in_pts2);
 
-    cv::imshow("Feature tracker", _image);
-}
+void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius=5, const cv::Scalar& _color = cv::Scalar(0, 0, 255), const int& _thickness=-1);
 
-inline void copyKPVector(const KeyPointVector& _kpv_from, KeyPointVector& _kpv_to)
-{
-	_kpv_to.clear();
-	for (auto kp : _kpv_from)
-		_kpv_to.push_back(kp);
-};
+void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255) );
 
-inline PointVector KPToP(const KeyPointVector& _kp_vec)
-{
-    PointVector pt_vec;
-    for (auto kp : _kp_vec)
-    	pt_vec.push_back(kp.pt);
-    return pt_vec;
-};
+void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255));
 
-void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2);
-void whoHasMoved(const PointVector& _pts1, const PointVector& _pts2, PointVector& _common_pts, PointVector& _new_in_pts2);
+void trimRoi(const int& _img_width, const int& _img_height, cv::Rect& _roi);
+
+void inflateRoi(const int& _inflation_rate, cv::Rect& _roi);
+
+void adaptRoi(const int& _inflation_rate, cv::Mat& _image_roi, cv::Mat _image, cv::Rect& _roi);
 
 } /* namespace vision_utils */