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 */