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_TABLETOP_COLOR_DIFFERENCE_LIKELIHOOD_H_
00038 #define JSK_PERCEPTION_TABLETOP_COLOR_DIFFERENCE_LIKELIHOOD_H_
00039
00040 #include <jsk_topic_tools/diagnostic_nodelet.h>
00041 #include <sensor_msgs/Image.h>
00042 #include <sensor_msgs/CameraInfo.h>
00043 #include <jsk_recognition_msgs/PolygonArray.h>
00044 #include <jsk_recognition_utils/tf_listener_singleton.h>
00045 #include <jsk_topic_tools/log_utils.h>
00046 #include <tf/message_filter.h>
00047 #include <message_filters/subscriber.h>
00048 #include <jsk_recognition_utils/geo/polygon.h>
00049 #include <jsk_perception/TabletopColorDifferenceLikelihoodConfig.h>
00050 #include <dynamic_reconfigure/server.h>
00051 #include <jsk_recognition_msgs/HistogramWithRangeBin.h>
00052 namespace jsk_perception
00053 {
00054 class TabletopColorDifferenceLikelihood: public jsk_topic_tools::DiagnosticNodelet
00055 {
00056 public:
00057 typedef TabletopColorDifferenceLikelihoodConfig Config;
00058 TabletopColorDifferenceLikelihood(): DiagnosticNodelet("TabletopColorDifferenceLikelihood") {}
00059 protected:
00060 virtual void onInit();
00061 virtual void subscribe();
00062 virtual void unsubscribe();
00063 virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg);
00064 virtual void imageCallback(const sensor_msgs::Image::ConstPtr& msg);
00065 virtual void polygonCallback(const jsk_recognition_msgs::PolygonArray::ConstPtr& msg);
00066 virtual void configCallback(Config& config, uint32_t level);
00067
00068 virtual void debugPolygonImage(const jsk_recognition_utils::CameraDepthSensor& model,
00069 cv::Mat& image,
00070 jsk_recognition_utils::Polygon::Ptr polygon,
00071 size_t pi) const;
00072
00078 inline virtual unsigned char computePixelDistance(const unsigned char from, const unsigned char to) const
00079 {
00080 if (cyclic_value_) {
00081 unsigned char diff = (unsigned char)std::abs((int)from - (int)to);
00082 unsigned char reverse_diff = pixel_max_value_ - diff;
00083 return (unsigned char)std::min(diff, reverse_diff);
00084 }
00085 else {
00086 return (unsigned char)std::abs((int)from - (int)to);
00087 }
00088 }
00089
00093 inline virtual unsigned char computePixelHistogramDistance(const unsigned char from,
00094 const std::vector<jsk_recognition_msgs::HistogramWithRangeBin>& bins)
00095 {
00096 unsigned char diff = 255;
00097 for (size_t i = 0; i < bins.size(); i++) {
00098 jsk_recognition_msgs::HistogramWithRangeBin bin = bins[i];
00099 if (bin.min_value < from && bin.max_value > from) {
00100 return 0;
00101 }
00102 else {
00103 unsigned char min_direction_distance = computePixelDistance(from, (unsigned char)bin.min_value);
00104 unsigned char max_direction_distance = computePixelDistance(from, (unsigned char)bin.max_value);
00105 diff = std::min(std::min(min_direction_distance, max_direction_distance),
00106 diff);
00107 }
00108 }
00109 return diff;
00110 }
00111
00112 boost::mutex mutex_;
00113 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00114 sensor_msgs::CameraInfo::ConstPtr latest_info_msg_;
00115 jsk_recognition_msgs::PolygonArray::ConstPtr latest_polygon_msg_;
00116 tf::TransformListener* tf_listener_;
00117 ros::Publisher pub_;
00118 ros::Publisher pub_debug_polygon_;
00119 ros::Publisher pub_debug_histogram_image_;
00120 ros::Subscriber sub_info_;
00121 ros::Subscriber sub_polygons_;
00122 message_filters::Subscriber<sensor_msgs::Image> sub_image_;
00123 boost::shared_ptr<tf::MessageFilter<sensor_msgs::Image> > tf_filter_;
00124 int tf_queue_size_;
00125 bool cyclic_value_;
00126 int pixel_max_value_;
00127 int pixel_min_value_;
00128 int bin_size_;
00129 double histogram_top_n_ratio_;
00130 private:
00131 };
00132 }
00133
00134 #endif