00001 #ifndef VSLAM_SYSTEM_ANY_DETECTOR_H
00002 #define VSLAM_SYSTEM_ANY_DETECTOR_H
00003 
00004 #include <opencv2/features2d/features2d.hpp>
00005 
00006 namespace vslam_system {
00007 
00008 class AnyDetector : public cv::FeatureDetector
00009 {
00010   typedef cv::Ptr<cv::FeatureDetector> DetectorPtr;
00011   
00012   DetectorPtr active_detector_;
00013 
00014 public:
00015   AnyDetector(const DetectorPtr& detector = new cv::FastFeatureDetector)
00016     : active_detector_(detector)
00017   {
00018   }
00019 
00021   virtual void detect( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask=cv::Mat() ) const
00022   {
00023     detectImpl(image, keypoints, mask);
00024   }
00025 
00026   template <class Config>
00027   void update(Config& config)
00028   {
00029     if (config.detector == "FAST") {
00030       active_detector_ = new cv::FastFeatureDetector(config.fast_threshold, config.fast_nonmax_suppression);
00031     }
00032     else if (config.detector == "Harris") {
00033       active_detector_ = new cv::GoodFeaturesToTrackDetector(config.harris_max_keypoints, config.harris_quality_level,
00034                                                              config.harris_min_distance, config.harris_block_size,
00035                                                              true, config.harris_k);
00036     }
00037     else if (config.detector == "Star") {
00039       active_detector_ = new cv::StarFeatureDetector(config.star_max_size, config.star_response_threshold,
00040                                                      config.star_line_threshold_projected,
00041                                                      config.star_line_threshold_binarized,
00042                                                      config.star_suppress_nonmax_size);
00043     }
00044     else if (config.detector == "SURF") {
00045       active_detector_ = new cv::SurfFeatureDetector(config.surf_hessian_threshold, config.surf_octaves,
00046                                                      config.surf_octave_layers);
00047     }
00048     else
00049       ROS_ERROR("Unknown detector '%s'", config.detector.c_str());
00051 
00052     if (config.grid_adapter) {
00053       active_detector_ = new cv::GridAdaptedFeatureDetector(active_detector_, config.grid_max_keypoints,
00054                                                             config.grid_rows, config.grid_cols);
00055     }
00056   }
00057 
00058 protected:
00059   virtual void detectImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask ) const
00060   {
00061     active_detector_->detect(image, keypoints, mask);
00062   }
00063 };
00064 
00065 } 
00066 
00067 #endif