adding structure for algorithm on optical flow pyramid lucas kanade

src/CMakeLists.txt

@@ -62,7 +62,10 @@ SET(sources
 	matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.cpp
 	matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2_load_yaml.cpp
 	matchers/flannbased/matcher_flannbased.cpp
-	matchers/flannbased/matcher_flannbased_load_yaml.cpp)
+	matchers/flannbased/matcher_flannbased_load_yaml.cpp
+	algorithms/algorithm_base.cpp
+	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp
+	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp)
 	
 # application header files
 SET(headers 
@@ -104,7 +107,10 @@ SET(headers
 	matchers/bruteforce_l1/matcher_bruteforce_l1.h
 	matchers/bruteforce_hamming/matcher_bruteforce_hamming.h
 	matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h
-	matchers/flannbased/matcher_flannbased.h)
+	matchers/flannbased/matcher_flannbased.h
+	algorithms/algorithm_factory.h
+	algorithms/algorithm_base.h
+	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h)
 
 # locate the necessary dependencies
 FIND_PACKAGE(Eigen3 REQUIRED)

src/algorithms/algorithm_base.cpp

+#include "algorithm_base.h"
+
+namespace vision_utils {
+
+AlgorithmBase::AlgorithmBase(void)
+{
+}
+
+AlgorithmBase::~AlgorithmBase(void)
+{
+}
+
+} /* namespace vision_utils */
+
+#include "algorithm_factory.h"
+
+namespace vision_utils
+{
+
+AlgorithmBasePtr setupAlgorithm(const std::string& _type, const std::string& _unique_name, const ParamsBasePtr& _params)
+{
+    AlgorithmBasePtr mat_ptr = AlgorithmFactory::get().create(_type, _unique_name, _params);
+    return mat_ptr;
+};
+
+#ifdef USING_YAML
+AlgorithmBasePtr setupAlgorithm(const std::string& _type, const std::string& _unique_name, const std::string& _filename_dot_yaml)
+{
+	ParamsBasePtr params_ptr = ParamsFactory::get().create(_type+" ALG", _filename_dot_yaml);
+	return setupAlgorithm(_type, _unique_name, params_ptr);
+}
+#endif
+
+} /* namespace vision_utils */

src/algorithms/algorithm_base.h

+#ifndef _ALGORITHM_BASE_H_
+#define _ALGORITHM_BASE_H_
+
+// vision_utils
+#include "../vision_utils.h"
+
+namespace vision_utils
+{
+
+/////////////////////////////////////////////////////////////////////////
+//      CLASS DEFINITIONS
+/////////////////////////////////////////////////////////////////////////
+
+// Create all pointers
+VU_PTR_TYPEDEFS(AlgorithmBase);
+VU_PTR_TYPEDEFS(AlgorithmParamsBase);
+
+/** \brief Class parameters
+ *
+ */
+struct AlgorithmParamsBase: public ParamsBase
+{};
+
+/** \brief base class for Algorithm base
+ *
+ * Derive from this class to create Algorithm base class parameters.
+ */
+class AlgorithmBase : public VUBase, public std::enable_shared_from_this<AlgorithmBase>
+{
+   public:
+
+        /**
+         * \brief Constructor without parameters
+         */
+        AlgorithmBase(void);
+
+        /**
+         * \brief Virtual destructor
+         */
+        virtual ~AlgorithmBase(void);
+
+        std::string getName(void);
+
+        AlgorithmParamsBasePtr getParams(void);
+
+        // Factory method
+        static AlgorithmBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);
+
+   protected:
+
+        std::string name_;
+
+        AlgorithmParamsBasePtr params_base_ptr_;
+
+        void setName(const std::string& _name);
+
+        virtual void defineAlgorithm(const ParamsBasePtr _params) = 0;
+};
+
+/*
+ * brief Retrieve object name
+ */
+inline std::string AlgorithmBase::getName(void) { return name_; }
+
+/*
+ * brief Set object name
+ */
+inline void AlgorithmBase::setName(const std::string& _name){ name_ = _name; }
+
+/*
+ * brief Retrieve object parameters
+ */
+inline AlgorithmParamsBasePtr AlgorithmBase::getParams(void) { return params_base_ptr_; }
+
+/*
+ * brief Setup and get the corresponding pointer
+ */
+AlgorithmBasePtr setupAlgorithm(const std::string& _type, const std::string& _unique_name, const ParamsBasePtr& _params);
+#ifdef USING_YAML
+AlgorithmBasePtr setupAlgorithm(const std::string& _type, const std::string& _unique_name, const std::string& _filename_dot_yaml);
+#endif
+
+} /* namespace vision_utils */
+
+#endif /* _ALGORITHM_BASE_H_ */

src/algorithms/algorithm_factory.h

+#ifndef _ALGORITHM_FACTORY_H_
+#define _ALGORITHM_FACTORY_H_
+
+namespace vision_utils
+{
+class AlgorithmBase;
+struct AlgorithmParamsBase;
+}
+
+#include "../factory.h"
+
+namespace vision_utils
+{
+
+/* Algorithm Factory */
+typedef Factory<AlgorithmBase,
+        const std::string&,
+        const ParamsBasePtr> AlgorithmFactory;
+
+template<>
+inline std::string AlgorithmFactory::getClass()
+{
+    return "AlgorithmFactory";
+}
+
+#define VU_REGISTER_ALGORITHM(AlgorithmType, AlgorithmName) \
+  namespace{ const bool AlgorithmName##Registered = \
+	AlgorithmFactory::get().registerCreator(AlgorithmType, AlgorithmName::create, true); }\
+
+} /* namespace vision_utils */
+
+#endif /* _ALGORITHM_FACTORY_H_ */

src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp

+#include "alg_opticalflowpyrlk.h"
+
+namespace vision_utils {
+
+AlgorithmOPTFLOWPYRLK::AlgorithmOPTFLOWPYRLK(void)
+{}
+
+AlgorithmOPTFLOWPYRLK::~AlgorithmOPTFLOWPYRLK(void)
+{}
+
+} /* namespace vision_utils */
+
+// Register in the AlgorithmsFactory
+namespace vision_utils
+{
+VU_REGISTER_ALGORITHM("OPTFLOWPYRLK", AlgorithmOPTFLOWPYRLK);
+} /* namespace vision_utils */

src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h

+#ifndef _ALGORITHM_OPTFLOWPYRLK_H_
+#define _ALGORITHM_OPTFLOWPYRLK_H_
+
+#include "../algorithm_base.h"
+#include "../algorithm_factory.h"
+
+// yaml-cpp library
+#ifdef USING_YAML
+	#include <yaml-cpp/yaml.h>
+#endif
+
+namespace vision_utils {
+
+// Create all pointers
+VU_PTR_TYPEDEFS(AlgorithmOPTFLOWPYRLK);
+VU_PTR_TYPEDEFS(AlgorithmParamsOPTFLOWPYRLK);
+
+/** \brief Class parameters
+ *
+ */
+struct AlgorithmParamsOPTFLOWPYRLK: public AlgorithmParamsBase
+{
+	// TODO: Add possible parameters
+};
+
+/** \brief DETECTOR class
+ *
+ */
+class AlgorithmOPTFLOWPYRLK : public AlgorithmBase {
+
+    public:
+		AlgorithmOPTFLOWPYRLK();
+        virtual ~AlgorithmOPTFLOWPYRLK(void);
+
+        // Factory method
+        static AlgorithmBasePtr create(const std::string& _unique_name, const ParamsBasePtr _params);
+
+    private:
+
+        void defineAlgorithm(const ParamsBasePtr _params);
+};
+
+/*
+ * brief Define detector
+ */
+inline void AlgorithmOPTFLOWPYRLK::defineAlgorithm(const ParamsBasePtr _params)
+{
+	params_base_ptr_ = std::static_pointer_cast<AlgorithmParamsBase>(_params);
+	AlgorithmParamsOPTFLOWPYRLKPtr params_ptr = std::static_pointer_cast<AlgorithmParamsOPTFLOWPYRLK>(_params);
+}
+
+/*
+ * brief Create object in factory
+ */
+inline AlgorithmBasePtr AlgorithmOPTFLOWPYRLK::create(const std::string& _unique_name, const ParamsBasePtr _params)
+{
+    AlgorithmOPTFLOWPYRLKPtr mat_ptr = std::make_shared<AlgorithmOPTFLOWPYRLK>();
+    mat_ptr->setName(_unique_name);
+    mat_ptr->defineAlgorithm(_params);
+    return mat_ptr;
+}
+
+} /* namespace vision_utils */
+
+#endif /* _ALGORITHM_OPTFLOWPYRLK_H_ */

src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp

+#include "alg_opticalflowpyrlk.h"
+
+#ifdef USING_YAML
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace vision_utils
+{
+
+namespace
+{
+
+static ParamsBasePtr createParamsOPTFLOWPYRLKAlgorithm(const std::string & _filename_dot_yaml)
+{
+	AlgorithmParamsOPTFLOWPYRLKPtr params_ptr = std::make_shared<AlgorithmParamsOPTFLOWPYRLK>();
+
+    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>() == "OPTFLOWPYRLK")
+        {
+        	// TODO: Add possible parameters
+        }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_matOPTFLOWPYRLK_params = ParamsFactory::get().registerCreator("OPTFLOWPYRLK ALG", createParamsOPTFLOWPYRLKAlgorithm);
+
+} /* namespace [unnamed] */
+
+} /* namespace vision_utils */
+
+#endif /* IF USING_YAML */
+

src/examples/CMakeLists.txt

@@ -4,10 +4,12 @@ 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)
 
 # link necessary libraries
 TARGET_LINK_LIBRARIES(test_factories ${PROJECT_NAME})
 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_matcher vision_utils)
\ No newline at end of file
+TARGET_LINK_LIBRARIES(test_algorithm vision_utils)
\ No newline at end of file

src/examples/test_algorithm.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/opticalflowpyrlk/alg_opticalflowpyrlk.h"
+
+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 = "ORB";
+    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================ 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");
+
+    if (alg_name.compare("OPTFLOWPYRLK") == 0)
+    	alg_ptr = std::static_pointer_cast<AlgorithmOPTFLOWPYRLK>(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;
+
+    cv::Mat frame_cur;
+
+    for (int nframe = 0; nframe < 1000; ++nframe)
+    {
+        // Get frame
+    	sen_ptr->getFrame(frame_cur);
+
+		// Detector
+        KeyPointVector kpts = det_ptr->detect(frame_cur);
+
+      	drawKeyPoints(frame_cur, kpts);
+
+      	// TODO: Algorithm calls
+
+       	cv::imshow(alg_ptr->getName(), frame_cur);
+       	cv::waitKey(1);
+   }
+
+    cv::destroyAllWindows();
+}

src/examples/test_matcher.cpp

@@ -157,6 +157,8 @@ int main(void)
     KeyPointVector kpts_old;
     cv::Mat descriptors_old;
 
+    KeyPointVector KPBuff;
+
     for (int nframe = 0; nframe < 1000; ++nframe)
     {
         // Get frame
@@ -202,7 +204,6 @@ int main(void)
             frame_matches = frame_cur.clone();
         	if  (!kpts_matched_prevf.empty() && !kpts_matched_curf.empty())
             {
-
             	// draw detections
             	drawKeyPoints(frame_old, kpts_old);
             	drawKeyPoints(frame_cur, kpts);

src/examples/yaml/OPTFLOWPYRLK.yaml

+sensor:
+  type: "USB_CAM"
+algorithm:
+  type: "OPTFLOWPYRLK"         

src/vision_utils.cpp

@@ -3,4 +3,30 @@
 namespace vision_utils
 {
 
+void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2)
+{
+	_new_in_kpts2.clear();
+	_common_kpts.clear();
+
+    for (int ii = 0; ii < _kpts2.size(); ++ii)
+    {
+      bool existing = false;
+      for (int jj = 0; jj < _kpts1.size(); ++jj)
+      {
+        cv::Point2f p1 = _kpts2[ii].pt;
+        cv::Point2f p2 = _kpts1[ii].pt;
+        if (p1.x-p2.x < 1.0 || p1.y-p2.y < 1.0)
+        {
+          existing = true;
+          break;
+        }
+      }
+      if (!existing) // New feature
+    	  _new_in_kpts2.push_back(_kpts2[ii]);
+      else
+    	  _common_kpts.push_back(_kpts2[ii]);
+    }
+}
+
 } /* namespace vision_utils */
+

src/vision_utils.h

@@ -122,12 +122,17 @@ 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));
-}
+};
+
+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));
+};
 
 inline std::vector<cv::Point2f> vecIntersec(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
 {
@@ -138,7 +143,7 @@ inline std::vector<cv::Point2f> vecIntersec(std::vector<cv::Point2f> v1, std::ve
   // Intersect
   std::set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(v3), LessPoints);
   return v3;
-}
+};
 
 inline std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vector<cv::Point2f> v2)
 {
@@ -148,26 +153,28 @@ inline std::vector<cv::Point2f> vecUnion(std::vector<cv::Point2f> v1, std::vecto
   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;
-}
+};
 
 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 (unsigned int ii = 0; ii < _kp_vec.size(); ++ii)
         cv::circle(_image, _kp_vec[ii].pt, _radius, _color, _thickness);
-}
+};
 
 inline void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec)
 {
     for (unsigned int ii = 0; ii < _kl_vec.size(); ++ii)
         cv::line(_image, _kl_vec[ii].getStartPoint(), _kl_vec[ii].getEndPoint(), cv::Scalar(128, 128, 255), 3);
-}
+};
 
 inline void copyKPVector(const KeyPointVector& kpv_from, KeyPointVector& kpv_to)
 {
 	kpv_to.clear();
 	for (int ii=0; ii<kpv_from.size(); ii++)
 		kpv_to.push_back(kpv_from[ii]);
-}
+};
+
+void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2);
 
 } /* namespace vision_utils */