diff --git a/doc/images/tesselationExample.png b/doc/images/tesselationExample.png new file mode 100644 index 0000000000000000000000000000000000000000..21801400c2257327da79b6d338e09889cc37ca20 Binary files /dev/null and b/doc/images/tesselationExample.png differ diff --git a/doc/images/tesselationGrid.png b/doc/images/tesselationGrid.png new file mode 100644 index 0000000000000000000000000000000000000000..6ce0ee485b9b308bbcc836827ea9de3f71eb05f8 Binary files /dev/null and b/doc/images/tesselationGrid.png differ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 175def8dfdc0fb24658197bec161593742c14e94..9bc4cc8d46dc83971dbc10ab2cea8243713c2fc2 100644 --- a/src/CMakeLists.txt +++ b/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) diff --git a/src/algorithms/activesearch/alg_activesearch.cpp b/src/algorithms/activesearch/alg_activesearch.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cdf5da25a1144a2a11bc69858e854dd6a1d4f438 --- /dev/null +++ b/src/algorithms/activesearch/alg_activesearch.cpp @@ -0,0 +1,95 @@ +#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 */ diff --git a/src/algorithms/activesearch/alg_activesearch.h b/src/algorithms/activesearch/alg_activesearch.h new file mode 100644 index 0000000000000000000000000000000000000000..c10b3f8f781464514f50107eb47a20a109c010d6 --- /dev/null +++ b/src/algorithms/activesearch/alg_activesearch.h @@ -0,0 +1,330 @@ +#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_ */ diff --git a/src/algorithms/activesearch/alg_activesearch_load_yaml.cpp b/src/algorithms/activesearch/alg_activesearch_load_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..48a24dd366d572f66ee1439fc8d4fc0d22d28ec1 --- /dev/null +++ b/src/algorithms/activesearch/alg_activesearch_load_yaml.cpp @@ -0,0 +1,50 @@ +#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 */ + diff --git a/src/algorithms/algorithm_base.h b/src/algorithms/algorithm_base.h index 13a4af29939298ecbd5198e89f8c054893b13fc1..28f3c3a9d1ca13a80bc387e77dab80ceab1bfd6f 100644 --- a/src/algorithms/algorithm_base.h +++ b/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); diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 96db02d97131727f7e27f553f128eda5f8c37ddd..27850e06558d6091af3b6f3f696c84c94ffd0ca2 100644 --- a/src/examples/CMakeLists.txt +++ b/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) \ No newline at end of file +TARGET_LINK_LIBRARIES(test_algorithm_opticalflowpyrlk vision_utils) +TARGET_LINK_LIBRARIES(test_algorithm_activesearch vision_utils) \ No newline at end of file diff --git a/src/examples/test_algorithm_activesearch.cpp b/src/examples/test_algorithm_activesearch.cpp new file mode 100644 index 0000000000000000000000000000000000000000..495282f408cbebda607783721c72442fe845f536 --- /dev/null +++ b/src/examples/test_algorithm_activesearch.cpp @@ -0,0 +1,167 @@ +#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(); +} diff --git a/src/examples/test_algorithm.cpp b/src/examples/test_algorithm_opticalflowpyrlk.cpp similarity index 92% rename from src/examples/test_algorithm.cpp rename to src/examples/test_algorithm_opticalflowpyrlk.cpp index 37457d2c4bfb04de109cedc5090e99d5697f4f26..585066f85ccd2e96f68b933cde06ff77a6e92411 100644 --- a/src/examples/test_algorithm.cpp +++ b/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; diff --git a/src/examples/yaml/ACTIVESEARCH.yaml b/src/examples/yaml/ACTIVESEARCH.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9b7954f52b537eb6d89f4df61cee73b8d0bbc2fd --- /dev/null +++ b/src/examples/yaml/ACTIVESEARCH.yaml @@ -0,0 +1,17 @@ +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