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