37 #ifndef JSK_PERCEPTION_TABLETOP_COLOR_DIFFERENCE_LIKELIHOOD_H_ 38 #define JSK_PERCEPTION_TABLETOP_COLOR_DIFFERENCE_LIKELIHOOD_H_ 41 #include <sensor_msgs/Image.h> 42 #include <sensor_msgs/CameraInfo.h> 43 #include <jsk_recognition_msgs/PolygonArray.h> 49 #include <jsk_perception/TabletopColorDifferenceLikelihoodConfig.h> 50 #include <dynamic_reconfigure/server.h> 51 #include <jsk_recognition_msgs/HistogramWithRangeBin.h> 57 typedef TabletopColorDifferenceLikelihoodConfig
Config;
63 virtual void infoCallback(
const sensor_msgs::CameraInfo::ConstPtr&
msg);
64 virtual void imageCallback(
const sensor_msgs::Image::ConstPtr& msg);
65 virtual void polygonCallback(
const jsk_recognition_msgs::PolygonArray::ConstPtr& msg);
81 unsigned char diff = (
unsigned char)std::abs((
int)from - (
int)to);
83 return (
unsigned char)std::min(diff, reverse_diff);
86 return (
unsigned char)std::abs((
int)from - (
int)to);
94 const std::vector<jsk_recognition_msgs::HistogramWithRangeBin>& bins)
96 unsigned char diff = 255;
97 for (
size_t i = 0; i < bins.size(); i++) {
98 jsk_recognition_msgs::HistogramWithRangeBin bin = bins[i];
99 if (bin.min_value < from && bin.max_value > from) {
103 unsigned char min_direction_distance =
computePixelDistance(from, (
unsigned char)bin.min_value);
104 unsigned char max_direction_distance =
computePixelDistance(from, (
unsigned char)bin.max_value);
105 diff = std::min(std::min(min_direction_distance, max_direction_distance),
sensor_msgs::CameraInfo::ConstPtr latest_info_msg_
virtual void polygonCallback(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
TabletopColorDifferenceLikelihoodConfig Config
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &msg)
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > tf_filter_
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
virtual void configCallback(Config &config, uint32_t level)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
TabletopColorDifferenceLikelihood()
double histogram_top_n_ratio_
virtual void unsubscribe()
ros::Publisher pub_debug_histogram_image_
virtual unsigned char computePixelHistogramDistance(const unsigned char from, const std::vector< jsk_recognition_msgs::HistogramWithRangeBin > &bins)
virtual unsigned char computePixelDistance(const unsigned char from, const unsigned char to) const
compute a distance between pixels. if cyclic_value_ is true, it take into account two direction...
ros::Publisher pub_debug_polygon_
virtual void debugPolygonImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image, jsk_recognition_utils::Polygon::Ptr polygon, size_t pi) const
ros::Subscriber sub_info_
tf::TransformListener * tf_listener_
ros::Subscriber sub_polygons_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
jsk_recognition_msgs::PolygonArray::ConstPtr latest_polygon_msg_