Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef JSK_PERCEPTION_COLOR_HISTOGRAM_LABEL_MATCH_H_
00038 #define JSK_PERCEPTION_COLOR_HISTOGRAM_LABEL_MATCH_H_
00039
00040 #include <jsk_topic_tools/diagnostic_nodelet.h>
00041 #include <sensor_msgs/Image.h>
00042 #include <jsk_recognition_msgs/ColorHistogram.h>
00043
00044 #include <message_filters/subscriber.h>
00045 #include <message_filters/synchronizer.h>
00046 #include <message_filters/sync_policies/exact_time.h>
00047 #include <message_filters/sync_policies/approximate_time.h>
00048 #include <message_filters/pass_through.h>
00049 #include <jsk_perception/ColorHistogramLabelMatchConfig.h>
00050 #include <dynamic_reconfigure/server.h>
00051
00052 #include <opencv2/opencv.hpp>
00053
00054 namespace jsk_perception
00055 {
00056 class ColorHistogramLabelMatch: public jsk_topic_tools::DiagnosticNodelet
00057 {
00058 public:
00059 typedef ColorHistogramLabelMatchConfig Config;
00060
00061 typedef message_filters::sync_policies::ApproximateTime<
00062 sensor_msgs::Image,
00063 sensor_msgs::Image,
00064 sensor_msgs::Image
00065 > SyncPolicy;
00066 typedef message_filters::sync_policies::ApproximateTime<
00067 sensor_msgs::Image,
00068 sensor_msgs::Image
00069 > SyncPolicyWithoutMask;
00070
00071 ColorHistogramLabelMatch(): DiagnosticNodelet("ColorHistogramLabelMatch") {}
00072 protected:
00073 virtual void onInit();
00074 virtual void subscribe();
00075 virtual void unsubscribe();
00076 virtual void match(
00077 const sensor_msgs::Image::ConstPtr& image_msg,
00078 const sensor_msgs::Image::ConstPtr& label_msg,
00079 const sensor_msgs::Image::ConstPtr& mask_msg);
00080 virtual void match(
00081 const sensor_msgs::Image::ConstPtr& image_msg,
00082 const sensor_msgs::Image::ConstPtr& label_msg);
00083 virtual void histogramCallback(
00084 const jsk_recognition_msgs::ColorHistogram::ConstPtr& histogram_msg);
00085 virtual bool isMasked(const cv::Mat& original_image,
00086 const cv::Mat& masked_image);
00087 virtual void getLabels(const cv::Mat& label, std::vector<int>& labels);
00088 virtual void getMaskImage(const cv::Mat& label_image,
00089 const int label,
00090 cv::Mat& mask);
00091 virtual double coefficients(const cv::Mat& ref_hist,
00092 const cv::Mat& target_hist);
00093 virtual void configCallback(Config &config, uint32_t level);
00094
00095 float max_value_;
00096 float min_value_;
00097 float coef_threshold_;
00098 float masked_coefficient_;
00099 int threshold_method_;
00100 bool use_mask_;
00101 boost::mutex mutex_;
00102 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00103 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
00104 boost::shared_ptr<message_filters::Synchronizer<SyncPolicyWithoutMask> > sync_wo_mask_;
00105 message_filters::Subscriber<sensor_msgs::Image> sub_image_;
00106 message_filters::Subscriber<sensor_msgs::Image> sub_label_;
00107 message_filters::Subscriber<sensor_msgs::Image> sub_mask_;
00108 int coefficient_method_;
00109 ros::Subscriber sub_histogram_;
00110 cv::Mat histogram_;
00111 ros::Publisher pub_debug_;
00112 ros::Publisher pub_coefficient_image_;
00113 ros::Publisher pub_mask_;
00114 ros::Publisher pub_result_;
00115 private:
00116
00117 };
00118 }
00119
00120 #endif