added first version of active search

src/CMakeLists.txt

@@ -65,7 +65,9 @@ SET(sources
 	matchers/flannbased/matcher_flannbased_load_yaml.cpp
 	algorithms/algorithm_base.cpp
 	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp
-	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp)
+	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp
+	algorithms/activesearch/alg_activesearch.cpp
+	algorithms/activesearch/alg_activesearch_load_yaml.cpp	)
 	
 # application header files
 SET(headers 
@@ -110,7 +112,8 @@ SET(headers
 	matchers/flannbased/matcher_flannbased.h
 	algorithms/algorithm_factory.h
 	algorithms/algorithm_base.h
-	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h)
+	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h
+	algorithms/activesearch/alg_activesearch.h)
 
 # locate the necessary dependencies
 FIND_PACKAGE(Eigen3 REQUIRED)

src/algorithms/activesearch/alg_activesearch.cpp

+#include "alg_activesearch.h"
+
+namespace vision_utils {
+
+AlgorithmACTIVESEARCH::AlgorithmACTIVESEARCH(void): initialized_(false)
+{}
+
+AlgorithmACTIVESEARCH::~AlgorithmACTIVESEARCH(void)
+{}
+
+void AlgorithmACTIVESEARCH::resizeImage(unsigned int _img_size_h, unsigned int _img_size_v)
+{
+    img_size_(0) = _img_size_h;
+    img_size_(1) = _img_size_v;
+    cell_size_(0) = _img_size_h / (grid_size_(0) - 1);
+    cell_size_(1) = _img_size_v / (grid_size_(1) - 1);
+    offset_ = -cell_size_ / 2;
+    renew();
+}
+
+// Functions to fill in cells
+bool AlgorithmACTIVESEARCH::pickEmptyCell(Eigen::Vector2i & _cell) {
+    int k = 0;
+    Eigen::Vector2i cell0;
+    for (int i = 1; i < grid_size_(0) - 1; i++) {
+        for (int j = 1; j < grid_size_(1) - 1; j++) {
+            cell0(0) = i;
+            cell0(1) = j;
+            if (projections_count_(i, j) == 0) {
+                empty_cells_tile_tmp_(0,k) = i; //may be done in a better way
+                empty_cells_tile_tmp_(1,k) = j;
+                k++;
+            }
+        }
+    }
+    if (k > 0) { // number of empty inner cells
+        int idx = rand() % k; // between 0 and k-1
+        _cell(0) = empty_cells_tile_tmp_(0, idx);
+        _cell(1) = empty_cells_tile_tmp_(1, idx);
+        return true;
+    }
+    else
+        return false;
+}
+
+/*
+ * Get cell origin (exact pixel)
+ */
+Eigen::Vector2i AlgorithmACTIVESEARCH::cellOrigin(const Eigen::Vector2i & _cell) {
+    Eigen::Vector2i cell0;
+    cell0(0) = offset_(0) + cell_size_(0) * _cell(0);
+    cell0(1) = offset_(1) + cell_size_(1) * _cell(1);
+    return cell0;
+}
+
+void AlgorithmACTIVESEARCH::cell2roi(const Eigen::Vector2i & _cell, cv::Rect & _roi) {
+    roi_coordinates_ = cellOrigin(_cell);
+    roi_coordinates_(0) += separation_;
+    roi_coordinates_(1) += separation_;
+    Eigen::Vector2i roi_size = cell_size_;
+    roi_size(0) -= 2 * separation_;
+    roi_size(1) -= 2 * separation_;
+    _roi.x = roi_coordinates_(0);
+    _roi.y = roi_coordinates_(1);
+    _roi.width = roi_size(0);
+    _roi.height = roi_size(1);
+}
+
+bool AlgorithmACTIVESEARCH::pickRoi(cv::Rect & _roi) {
+
+    Eigen::Vector2i cell;
+    if (pickEmptyCell(cell)) {
+        cell2roi(cell, _roi);
+        return true;
+    }
+    else
+        return false;
+}
+
+void AlgorithmACTIVESEARCH::blockCell(const cv::Rect & _roi)
+{
+    Eigen::Vector2i pix;
+    pix(0) = _roi.x+_roi.width/2;
+    pix(1) = _roi.y+_roi.height/2;
+    Eigen::Vector2i cell = coords2cell(pix(0), pix(1));
+    projections_count_(cell(0), cell(1)) = -1;
+}
+
+} /* namespace vision_utils */
+
+// Register in the AlgorithmsFactory
+namespace vision_utils
+{
+VU_REGISTER_ALGORITHM("ACTIVESEARCH", AlgorithmACTIVESEARCH);
+} /* namespace vision_utils */

src/algorithms/activesearch/alg_activesearch.h

+#ifndef _ALGORITHM_ACTIVESEARCH_H_
+#define _ALGORITHM_ACTIVESEARCH_H_
+
+#include "../algorithm_base.h"
+#include "../algorithm_factory.h"
+
+// yaml-cpp library
+#ifdef USING_YAML
+	#include <yaml-cpp/yaml.h>
+#endif
+
+//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
+VU_PTR_TYPEDEFS(AlgorithmACTIVESEARCH);
+VU_PTR_TYPEDEFS(AlgorithmParamsACTIVESEARCH);
+
+/** \brief Class parameters
+ *
+ */
+struct AlgorithmParamsACTIVESEARCH: public AlgorithmParamsBase
+{
+	int img_size_h = 0;		// Horizontal image size, in pixels.
+	int img_size_v = 0;		// Vertical image size.
+	int n_cells_h = 0;		// Horizontal number of cells per image width.
+	int n_cells_v = 0;		// Vertical number of cells per image height.
+	int margin = 0;			// Minimum separation between existing and new points.
+	int separation = 0;		// Minimum separation to the edge of the image.
+};
+
+/**
+ * \brief Active search tesselation grid.
+ *
+ * \author jsola, dinesh
+ *
+ * This class implements a tesselation grid for achieving active search
+ * behavior in landmark initialization.
+ *
+ * The grid defines a set of cells in the image.
+ * The idea is to count the number of projected landmarks per grid cell,
+ * and use one randomly chosen cell that is empty
+ * for feature detection and landmark initialization.
+ * This guarantees that the system will automatically populate all the
+ * regions of the image.
+ *
+ * The feature density can be controlled by
+ * adjusting the grid's number of cells.
+ * Typically, use grids of 5x5 to 18x12 cells. Try to make reasonably square cells.
+ *
+ * This class implements a few interesting features:
+ * - The grid can be randomly re-positioned at each frame to avoid dead zones at the cell edges.
+ * - Only the inner cells are activated for feature detection to avoid reaching the image edges.
+ * - The region of interest (ROI) associated with a particular cell is shrinked with a parametrizable amount
+ *   to guarantee a minimum 'separation' between existing and new features.
+ * - The region of interest is ensured to lie at a distance from the image boundaries, defined by the parameter 'margin'.
+ *
+ * The blue and green grids in the figure below represent the grid
+ * at two different offsets, corresponding to two different frames.
+ *
+ *   \image html tesselationGrid.png "The tesselation grid used for active feature detection and initialization"
+ *
+ * This second figure shows a typical situation that we use to explain the basic mechanism.
+ *
+ *   \image html tesselationExample.png "A typical configuration of the tesselation grid"
+ *
+ * Observe the figure and use the following facts as an operation guide:
+ * - The grid is offset by a fraction of a cell size.
+ *     - use renew() at each frame to clear the grid and set a new offset.
+ * - Projected landmarks are represented by red dots.
+ *     - After projection, use hitCell() to add a new dot to the grid.
+ * - Cells with projected landmarks inside are 'occupied'.
+ * - Only the inner cells (thick blue rectangle) are considered for Region of Interest (ROI) extraction.
+ * - One cell is chosen randomly among those that are empty.
+ *     - Use pickRoi() to obtain an empty ROI for initialization.
+ * - The ROI is smaller than the cell to guarantee a minimum feature separation.
+ *     - Use the optional 'separation' parameter at construction time to control this separation.
+ * - A new feature is to be be searched inside this ROI.
+ * - If there is no feature found in this ROI, call blockCell() function to avoid searching in this area again.
+ * - If you need to search more than one feature per frame, proceed like this:
+ *     - Call pickRoi().
+ *     - Try to detect a Feature in ROI.
+ *     - If successful detection
+ *         - add the detected pixel with hitCell().
+ *     - Else
+ *         - block the cell with blockCell().
+ *     - Repeat these steps for each feature to be searched.
+ *
+ * An example can be found in src/examples/test_algorithm_activesearch.cpp. To run it execute ./test_algorithm_activesearch from the bin directory.
+ */
+class AlgorithmACTIVESEARCH : public AlgorithmBase {
+
+    public:
+
+		AlgorithmACTIVESEARCH();
+        virtual ~AlgorithmACTIVESEARCH(void);
+
+        // Factory method
+        static AlgorithmBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);
+
+        AlgorithmParamsBasePtr getParams(void);
+
+        void setParams(AlgorithmParamsACTIVESEARCHPtr _params);
+
+        /**
+          * \brief Re-set the image size
+          * \param _img_size_h horizontal image size, in pixels.
+          * \param _img_size_v vertical image size.
+          */
+         void resizeImage(unsigned int _img_size_h, unsigned int _img_size_v);
+
+         /** \brief Clear grid.
+          *
+          * Sets all cell counters to zero.
+          */
+         void clear();
+
+         /**
+          * \brief Clear grid and position it at a new random location.
+          *
+          * Sets all cell counters to zero and sets a new random grid position.
+          */
+         void renew();
+
+         /**
+          * \brief Add a projected pixel to the grid.
+          * \param _x the x-coordinate of the pixel to add.
+          * \param _y the y-coordinate of the pixel to add.
+          */
+         template<typename Scalar>
+         void hitCell(const Scalar _x, const Scalar _y);
+
+         /**
+          * \brief Add a projected pixel to the grid.
+          * \param _pix the pixel to add as an Eigen 2-vector with any Scalar type.
+          */
+         template<typename Scalar>
+         void hitCell(const Eigen::Matrix<Scalar, 2, 1>& _pix);
+
+         /**
+          * \brief Add a projected pixel to the grid.
+          * \param _pix the pixel to add as a cv::KeyPoint.
+          */
+         void hitCell(const cv::KeyPoint& _pix);
+
+         /**
+          * \brief Get ROI of a random empty cell.
+          * \param _roi the resulting ROI
+          * \return true if ROI exists.
+          */
+         bool pickRoi(cv::Rect & _roi);
+
+         /**
+          * \brief Call this after pickRoi if no point was found in the roi
+          * in order to avoid searching again in it.
+          * \param _roi the ROI where nothing was found
+          */
+         void blockCell(const cv::Rect & _roi);
+
+    private:
+
+        bool initialized_;
+
+        AlgorithmParamsACTIVESEARCHPtr params_ptr_;
+
+        cv::Mat frame_gray_prev_;
+
+        void defineAlgorithm(const ParamsBasePtr _params);
+
+        // TODO: parameters?
+        Eigen::Vector2i img_size_;
+        Eigen::Vector2i grid_size_;
+        Eigen::Vector2i cell_size_;
+        Eigen::Vector2i offset_;
+        Eigen::Vector2i roi_coordinates_;
+        Eigen::MatrixXi projections_count_;
+        Eigen::MatrixXi empty_cells_tile_tmp_;
+        int separation_;
+        int margin_;
+
+        /**
+         * \brief Get cell corresponding to pixel
+         */
+        template<typename Scalar>
+        Eigen::Vector2i coords2cell(const Scalar _x, const Scalar _y);
+
+        /**
+         * \brief Get cell origin (exact pixel)
+         */
+        Eigen::Vector2i cellOrigin(const Eigen::Vector2i & _cell);
+
+        /**
+         * \brief Get cell center (can be decimal if size of cell is an odd number of pixels)
+         */
+        Eigen::Vector2i cellCenter(const Eigen::Vector2i& _cell);
+
+        /**
+         * \brief Get one random empty cell
+         */
+        bool pickEmptyCell(Eigen::Vector2i & _cell);
+
+        /**
+         * \brief Get the region of interest, reduced by a margin.
+         */
+        void cell2roi(const Eigen::Vector2i & _cell, cv::Rect& _roi);
+};
+
+/*
+ * brief Retrieve object parameters
+ */
+inline AlgorithmParamsBasePtr AlgorithmACTIVESEARCH::getParams(void) { return params_ptr_; }
+
+/*
+ * brief Set object parameters
+ */
+inline void AlgorithmACTIVESEARCH::setParams(AlgorithmParamsACTIVESEARCHPtr _params)
+{	params_ptr_ = _params; }
+
+/*
+ * brief Define detector
+ */
+inline void AlgorithmACTIVESEARCH::defineAlgorithm(const ParamsBasePtr _params)
+{
+	params_base_ptr_ = std::static_pointer_cast<AlgorithmParamsBase>(_params);
+	params_ptr_ = std::static_pointer_cast<AlgorithmParamsACTIVESEARCH>(_params);
+
+	if (params_ptr_->img_size_h == 0 || params_ptr_->img_size_v == 0)
+		std::cerr << "[Algorithm ACTIVESEARCH]: Image size not set. Current values [" << params_ptr_->img_size_h << "x" << params_ptr_->img_size_v << "]" << std::endl;
+	if (params_ptr_->n_cells_h == 0 || params_ptr_->n_cells_v == 0)
+		std::cerr << "[Algorithm ACTIVESEARCH]: number of cells not set. Current values [" << params_ptr_->n_cells_h << "," << params_ptr_->n_cells_v << "]" << std::endl;
+
+	// Set internal variables
+    separation_ = params_ptr_->separation;
+    margin_ = params_ptr_->margin;
+    projections_count_.resize(params_ptr_->n_cells_h + 1, params_ptr_->n_cells_v + 1);
+    empty_cells_tile_tmp_.resize(2, (params_ptr_->n_cells_h + 1) * (params_ptr_->n_cells_v + 1));
+    img_size_(0) = params_ptr_->img_size_h;
+    img_size_(1) = params_ptr_->img_size_v;
+    grid_size_(0) = params_ptr_->n_cells_h + 1;
+    grid_size_(1) = params_ptr_->n_cells_v + 1;
+    cell_size_(0) = params_ptr_->img_size_h / params_ptr_->n_cells_h;
+    cell_size_(1) = params_ptr_->img_size_v / params_ptr_->n_cells_v;
+    offset_ = -cell_size_ / 2;
+    renew();
+}
+
+/*
+ * brief Create object in factory
+ */
+inline AlgorithmBasePtr AlgorithmACTIVESEARCH::create(const std::string& _unique_name, const ParamsBasePtr _params)
+{
+    AlgorithmACTIVESEARCHPtr mat_ptr = std::make_shared<AlgorithmACTIVESEARCH>();
+    mat_ptr->setName(_unique_name);
+    mat_ptr->defineAlgorithm(_params);
+    return mat_ptr;
+}
+
+inline void AlgorithmACTIVESEARCH::clear()
+{
+    projections_count_.setZero();
+}
+
+inline void AlgorithmACTIVESEARCH::renew()
+{
+    offset_(0) = -(margin_ + rand() % (cell_size_(0) - 2 * margin_)); // from -margin to -(cellSize(0)-margin)
+    offset_(1) = -(margin_ + rand() % (cell_size_(1) - 2 * margin_)); // from -margin to -(cellSize(0)-margin)
+    clear();
+}
+
+inline void AlgorithmACTIVESEARCH::hitCell(const cv::KeyPoint& _pix)
+{
+    hitCell(_pix.pt.x, _pix.pt.y);
+}
+
+/**
+ * \brief Add a projected pixel to the grid.
+ * \param _pix the pixel to add as an Eigen 2-vector.
+ */
+template<typename Scalar>
+inline void AlgorithmACTIVESEARCH::hitCell(const Eigen::Matrix<Scalar, 2, 1>& _pix)
+{
+    hitCell(_pix(0), _pix(1));
+}
+
+/**
+ * \brief Add a projected pixel to the grid.
+ * \param _x the x-coordinate of the pixel to add.
+ * \param _y the y-coordinate of the pixel to add.
+ */
+template<typename Scalar>
+inline void AlgorithmACTIVESEARCH::hitCell(const Scalar _x, const Scalar _y)
+{
+    Eigen::Vector2i cell = coords2cell(_x, _y);
+    if (cell(0) < 0 || cell(1) < 0 || cell(0) >= grid_size_(0) || cell(1) >= grid_size_(1))
+        return;
+
+    if (projections_count_(cell(0), cell(1)) == -1)
+        projections_count_(cell(0), cell(1)) = 0;
+
+    projections_count_(cell(0), cell(1))++;
+}
+
+/**
+ * Get cell corresponding to pixel
+ */
+template<typename Scalar>
+inline Eigen::Vector2i AlgorithmACTIVESEARCH::coords2cell(const Scalar _x, const Scalar _y)
+{
+    Eigen::Vector2i cell;
+    cell(0) = (_x - offset_(0)) / cell_size_(0);
+    cell(1) = (_y - offset_(1)) / cell_size_(1);
+    return cell;
+}
+
+inline Eigen::Vector2i AlgorithmACTIVESEARCH::cellCenter(const Eigen::Vector2i& _cell)
+{
+    return cellOrigin(_cell) + cell_size_ / 2;
+}
+
+} /* namespace vision_utils */
+
+#endif /* _ALGORITHM_ACTIVESEARCH_H_ */

src/algorithms/activesearch/alg_activesearch_load_yaml.cpp

+#include "alg_activesearch.h"
+
+#ifdef USING_YAML
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace vision_utils
+{
+
+namespace
+{
+
+static ParamsBasePtr createParamsACTIVESEARCHAlgorithm(const std::string & _filename_dot_yaml)
+{
+	AlgorithmParamsACTIVESEARCHPtr params_ptr = std::make_shared<AlgorithmParamsACTIVESEARCH>();
+
+    using std::string;
+    using YAML::Node;
+    Node yaml_params = YAML::LoadFile(_filename_dot_yaml);
+    if (!yaml_params.IsNull())
+    {
+        Node d_yaml = yaml_params["algorithm"];
+        if(d_yaml["type"].as<string>() == "ACTIVESEARCH")
+        {
+        	params_ptr->img_size_h = d_yaml["img size h"].as<int>();
+        	params_ptr->img_size_v = d_yaml["img size v"].as<int>();
+        	params_ptr->n_cells_h = d_yaml["num cells h"].as<int>();
+        	params_ptr->n_cells_v = d_yaml["num cells v"].as<int>();
+        	params_ptr->margin = d_yaml["margin"].as<int>();
+        	params_ptr->separation = d_yaml["separation"].as<int>();
+        }else
+        {
+            std::cerr << "Bad configuration file. Wrong type " << d_yaml["type"].as<string>() << std::endl;
+            return nullptr;
+        }
+    }
+
+	return params_ptr;
+}
+
+// Register in the SensorFactory
+const bool registered_matACTIVESEARCH_params = ParamsFactory::get().registerCreator("ACTIVESEARCH ALG", createParamsACTIVESEARCHAlgorithm);
+
+} /* namespace [unnamed] */
+
+} /* namespace vision_utils */
+
+#endif /* IF USING_YAML */
+

src/algorithms/algorithm_base.h

@@ -43,7 +43,9 @@ class AlgorithmBase : public VUBase, public std::enable_shared_from_this<Algorit
 
         virtual AlgorithmParamsBasePtr getParams(void) = 0;
 
-        virtual void compute(cv::Mat& frame, PointVector& pts_prev, PointVector& pts_matched_in_prev, PointVector& pts_matched_in_frame) = 0;
+        virtual void setParams(AlgorithmParamsBasePtr _params) {};
+
+        virtual void compute(cv::Mat& frame, PointVector& pts_prev, PointVector& pts_matched_in_prev, PointVector& pts_matched_in_frame) {};
 
         // Factory method
         static AlgorithmBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);

src/examples/CMakeLists.txt

@@ -4,7 +4,8 @@ ADD_EXECUTABLE(test_sensor test_sensor.cpp)
 ADD_EXECUTABLE(test_detector test_detector.cpp)
 ADD_EXECUTABLE(test_descriptor test_descriptor.cpp)
 ADD_EXECUTABLE(test_matcher test_matcher.cpp)
-ADD_EXECUTABLE(test_algorithm test_algorithm.cpp)
+ADD_EXECUTABLE(test_algorithm_opticalflowpyrlk test_algorithm_opticalflowpyrlk.cpp)
+ADD_EXECUTABLE(test_algorithm_activesearch test_algorithm_activesearch.cpp)
 
 # link necessary libraries
 TARGET_LINK_LIBRARIES(test_factories ${PROJECT_NAME})
@@ -12,4 +13,5 @@ TARGET_LINK_LIBRARIES(test_sensor vision_utils)
 TARGET_LINK_LIBRARIES(test_detector vision_utils)
 TARGET_LINK_LIBRARIES(test_descriptor vision_utils)
 TARGET_LINK_LIBRARIES(test_matcher vision_utils)
-TARGET_LINK_LIBRARIES(test_algorithm vision_utils)
+TARGET_LINK_LIBRARIES(test_algorithm_opticalflowpyrlk vision_utils)
\ No newline at end of file
+TARGET_LINK_LIBRARIES(test_algorithm_activesearch vision_utils)
\ No newline at end of file

src/examples/test_algorithm_activesearch.cpp

+#include <iostream>
+#include <iomanip>
+#include <cstdlib>
+
+#include "../vision_utils.h"
+
+// Sensors
+#include "../sensors/usb_cam/usb_cam.h"
+
+// Detectors
+#include "../detectors/orb/detector_orb.h"
+#include "../detectors/fast/detector_fast.h"
+#include "../detectors/sift/detector_sift.h"
+#include "../detectors/surf/detector_surf.h"
+#include "../detectors/brisk/detector_brisk.h"
+#include "../detectors/mser/detector_mser.h"
+#include "../detectors/gftt/detector_gftt.h"
+#include "../detectors/harris/detector_harris.h"
+#include "../detectors/sbd/detector_sbd.h"
+#include "../detectors/kaze/detector_kaze.h"
+#include "../detectors/akaze/detector_akaze.h"
+#include "../detectors/agast/detector_agast.h"
+
+// Algorithms
+#include "../algorithms/activesearch/alg_activesearch.h"
+
+#define MIN_GOOD_POINTS 10
+
+int main(void)
+{
+    using namespace vision_utils;
+    using std::shared_ptr;
+    using std::make_shared;
+    using std::static_pointer_cast;
+
+    // YAML file with parameters
+    std::string path_yaml_file = "/src/examples/yaml";
+
+    // Root dir path
+    std::string vu_root = _VU_ROOT_DIR;
+
+    // Setup camera sensor by default
+    SensorBasePtr sen_b_ptr = setupSensor("USB_CAM", "CAMERA_test", vu_root + path_yaml_file + "/FAST.yaml"); // Any YAML with sensor type setup correctly
+    SensorCameraPtr sen_ptr = std::static_pointer_cast<SensorCamera>(sen_b_ptr);
+
+    std::string def_detector = "HARRIS";
+    std::cout << std::endl << "Which DETECTOR do you want to test? Type one of the registered names  [default: " << def_detector << "]: ";
+    std::string det_name = readFromUser(def_detector);
+
+    DetectorBasePtr det_ptr = setupDetector(det_name, det_name + " detector", vu_root + path_yaml_file + "/" + det_name + ".yaml");
+
+    if (det_name.compare("ORB") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorORB>(det_ptr);
+    else if (det_name.compare("FAST") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorFAST>(det_ptr);
+    else if (det_name.compare("SIFT") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorSIFT>(det_ptr);
+    else if (det_name.compare("SURF") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorSURF>(det_ptr);
+    else if (det_name.compare("BRISK") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorBRISK>(det_ptr);
+    else if (det_name.compare("MSER") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorMSER>(det_ptr);
+    else if (det_name.compare("GFTT") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorGFTT>(det_ptr);
+    else if (det_name.compare("HARRIS") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorHARRIS>(det_ptr);
+    else if (det_name.compare("SBD") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorSBD>(det_ptr);
+    else if (det_name.compare("KAZE") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorKAZE>(det_ptr);
+    else if (det_name.compare("AKAZE") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorAKAZE>(det_ptr);
+    else if (det_name.compare("AGAST") == 0)
+    	det_ptr = std::static_pointer_cast<DetectorAGAST>(det_ptr);
+
+    std::cout << "\n================ ACTIVE SEARCH TEST  =================" << std::endl;
+
+    std::string def_alg = "ACTIVESEARCH";
+
+    AlgorithmBasePtr alg_ptr = setupAlgorithm(def_alg, def_alg + " matcher", vu_root + path_yaml_file + "/" + def_alg + ".yaml");
+
+   	alg_ptr = std::static_pointer_cast<AlgorithmACTIVESEARCH>(alg_ptr);
+
+    std::cout << std::endl << "... Testing " << det_ptr->getName() << " with " <<  alg_ptr->getName() << " ..." << std::endl;
+
+    // Open camera
+    sen_ptr->open(0);
+
+    cv::startWindowThread();
+    cv::namedWindow(alg_ptr->getName(), cv::WINDOW_NORMAL);
+
+    // The following line is used to remove the OpenCV "init done" from the terminal
+    std::cout << "\e[A" << "         " << std::endl;
+
+    // get first frame to set parameters correctly
+    cv::Mat frame;
+    sen_ptr->getFrame(frame);
+	AlgorithmParamsACTIVESEARCHPtr params_ptr;
+	params_ptr = std::static_pointer_cast<AlgorithmParamsACTIVESEARCH>(alg_ptr->getParams());
+	params_ptr->img_size_h = frame.cols;
+	params_ptr->img_size_v = frame.rows;
+	alg_ptr->setParams(params_ptr);
+	std::cout << "[" << params_ptr->img_size_h << "x" << params_ptr->img_size_v << "]" << std::endl;
+
+
+//    PointVector pts_prev;
+//
+//    bool full_frame_detection = true;
+
+    for (int nframe = 0; nframe < 1000; ++nframe)
+    {
+        cv::Mat frame_cur;
+
+        // Get frame
+    	sen_ptr->getFrame(frame_cur);
+
+//    	if (full_frame_detection)
+//        {
+//            full_frame_detection = false;
+//
+//    	    // Detect features in current frame (and draw them in current frame).
+//            KeyPointVector kpts = det_ptr->detect(frame_cur);
+//
+//            // Check new features
+//            PointVector pts = KPToP(kpts);
+//            PointVector pts_new;
+//            PointVector pts_existing;
+//            whoHasMoved(pts_prev, pts, pts_existing, pts_new);
+//
+//            // Add new features to the buffer
+//            pts_prev.insert(pts_prev.end(),pts_new.begin(),pts_new.end());
+//            std::sort(pts_prev.begin(), pts_prev.end(), LessPoints);
+//
+//            // Colour new feature detections
+//            for (int ii = 0; ii < pts_new.size(); ++ii)
+//                cv::circle(frame_cur, pts_new[ii], 5, cv::Scalar(0,255,0), -1);
+//            // Colour matched features
+//            for (int ii = 0; ii < pts_existing.size(); ++ii)
+//                cv::circle(frame_cur, pts_existing[ii], 5, cv::Scalar(128,128,0), -1);
+//        }
+//    	else
+//    	{
+//    	    // Algorithm call
+//    	    PointVector pts_matched_in_frame;
+//    	    PointVector pts_matched_in_prev;
+//    	    alg_ptr->compute(frame_cur, pts_prev, pts_matched_in_prev, pts_matched_in_frame);
+//
+//    	    // If number of tracked features decreases, rescan and get new features
+//    	    if (pts_matched_in_frame.size() < MIN_GOOD_POINTS)
+//    	        full_frame_detection = true;
+//
+//    	    // Draw optical flow
+//    	    for(size_t ii=0; ii<pts_matched_in_frame.size(); ii++)
+//    	        cv::line(frame_cur, pts_matched_in_prev[ii], pts_matched_in_frame[ii], cv::Scalar(255,0,0), 10);
+//
+//    	    // update features found in current frame
+//    	    pts_prev = pts_matched_in_frame;
+//            std::sort(pts_prev.begin(), pts_prev.end(), LessPoints);
+//    	}
+
+   	    cv::imshow(alg_ptr->getName(), frame_cur);
+       	cv::waitKey(1);
+   }
+
+    cv::destroyAllWindows();
+}

src/examples/test_algorithm.cpp → src/examples/test_algorithm_opticalflowpyrlk.cpp

@@ -77,13 +77,10 @@ int main(void)
     std::cout << "\n================ OPTICAL FLOW TEST  =================" << std::endl;
 
     std::string def_alg = "OPTFLOWPYRLK";
-    std::cout << std::endl << "Which MATCHER do you want to test? Type one of the registered names  [default: " << def_alg << "]: ";
-    std::string alg_name = readFromUser(def_alg);
 
-    AlgorithmBasePtr alg_ptr = setupAlgorithm(alg_name, alg_name + " matcher", vu_root + path_yaml_file + "/" + alg_name + ".yaml");
+    AlgorithmBasePtr alg_ptr = setupAlgorithm(def_alg, def_alg + " matcher", vu_root + path_yaml_file + "/" + def_alg + ".yaml");
 
-    if (alg_name.compare("OPTFLOWPYRLK") == 0)
-    	alg_ptr = std::static_pointer_cast<AlgorithmOPTFLOWPYRLK>(alg_ptr);
+   	alg_ptr = std::static_pointer_cast<AlgorithmOPTFLOWPYRLK>(alg_ptr);
 
     std::cout << std::endl << "... Testing " << det_ptr->getName() << " with " <<  alg_ptr->getName() << " ..." << std::endl;
 

src/examples/yaml/ACTIVESEARCH.yaml

+sensor:
+  type: "USB_CAM"
+detector:
+  type: "HARRIS"
+  maxCorners: 1000
+  qualityLevel: 0.01
+  minDistance: 10
+  blockSize: 3
+  k: 0.04
+algorithm:
+  type: "ACTIVESEARCH"   
+  img size h: 640
+  img size v: 480
+  num cells h: 10
+  num cells v: 10
+  margin: 0
+  separation: 0
\ No newline at end of file