55 DiagnosticNodelet::onInit();
60 b_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
61 *
pnh_,
"blue_histogram", 1);
62 g_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
63 *
pnh_,
"green_histogram", 1);
64 r_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
65 *
pnh_,
"red_histogram", 1);
66 h_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
67 *
pnh_,
"hue_histogram", 1);
68 s_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
69 *
pnh_,
"saturation_histogram", 1);
70 i_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
71 *
pnh_,
"intensity_histogram", 1);
73 *
pnh_,
"input_image", 1);
75 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
76 dynamic_reconfigure::Server<Config>::CallbackType
f =
78 srv_->setCallback (f);
88 names.push_back(
"screenrectangle");
90 = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
92 sync_->registerCallback(boost::bind(
99 names.push_back(
"mask");
125 jsk_recognition_msgs::ColorHistogram& msg)
127 msg.histogram.clear();
128 for (
int i = 0; i < size; i++) {
129 float val = hist.at<
float>(0, i);
130 msg.histogram.push_back(val);
136 const std_msgs::Header& header)
138 float range[] = { 0, 256 } ;
139 const float* histRange = { range };
140 cv::MatND b_hist, g_hist, r_hist;
141 bool uniform =
true;
bool accumulate =
false;
142 std::vector<cv::Mat> bgr_planes;
143 split(bgr_image, bgr_planes);
145 cv::calcHist(&bgr_planes[0], 1, 0, mask, b_hist, 1, &
b_hist_size_,
146 &histRange, uniform, accumulate);
147 cv::calcHist(&bgr_planes[1], 1, 0, mask, g_hist, 1, &
g_hist_size_,
148 &histRange, uniform, accumulate);
149 cv::calcHist(&bgr_planes[2], 1, 0, mask, r_hist, 1, &
r_hist_size_,
150 &histRange, uniform, accumulate);
152 jsk_recognition_msgs::ColorHistogram b_histogram;
153 b_histogram.header = header;
157 jsk_recognition_msgs::ColorHistogram g_histogram;
158 g_histogram.header = header;
162 jsk_recognition_msgs::ColorHistogram r_histogram;
163 r_histogram.header = header;
169 const std_msgs::Header& header)
175 const std_msgs::Header& header)
182 const std_msgs::Header& header)
185 cv::cvtColor(bgr_image, hsi_image, CV_BGR2HSV);
187 float range[] = { 0, 256 } ;
188 const float* histRange = { range };
189 float h_range[] = { 0, 180 } ;
190 const float* h_histRange = { h_range };
191 cv::MatND h_hist, s_hist, i_hist;
192 bool uniform =
true;
bool accumulate =
false;
193 std::vector<cv::Mat> hsi_planes;
194 split(hsi_image, hsi_planes);
196 cv::calcHist(&hsi_planes[0], 1, 0, mask, h_hist, 1, &
h_hist_size_,
197 &h_histRange, uniform, accumulate);
198 cv::calcHist(&hsi_planes[1], 1, 0, mask, s_hist, 1, &
s_hist_size_,
199 &histRange, uniform, accumulate);
200 cv::calcHist(&hsi_planes[2], 1, 0, mask, i_hist, 1, &
i_hist_size_,
201 &histRange, uniform, accumulate);
203 jsk_recognition_msgs::ColorHistogram h_histogram;
204 h_histogram.header = header;
208 jsk_recognition_msgs::ColorHistogram s_histogram;
209 s_histogram.header = header;
213 jsk_recognition_msgs::ColorHistogram i_histogram;
214 i_histogram.header = header;
221 const sensor_msgs::Image::ConstPtr& image,
222 const geometry_msgs::PolygonStamped::ConstPtr& rectangle)
230 geometry_msgs::Point32 point0 = rectangle->polygon.points[0];
231 geometry_msgs::Point32 point1 = rectangle->polygon.points[1];
232 int min_x = std::max(0.0
f, std::min(point0.x, point1.x));
233 int min_y = std::max(0.0
f, std::min(point0.y, point1.y));
234 int max_x = std::min(std::max(point0.x, point1.x), (
float)image->width);
235 int max_y = std::min(std::max(point0.y, point1.y), (
float)image->height);
236 cv::Rect roi(min_x, min_y, max_x - min_x, max_y - min_y);
237 cv::Mat bgr_image, roi_image;
238 roi_image = cv_ptr->image(roi);
240 cv::cvtColor(roi_image, bgr_image, CV_RGB2BGR);
243 roi_image.copyTo(bgr_image);
260 const sensor_msgs::Image::ConstPtr& image,
261 const sensor_msgs::Image::ConstPtr& mask_image)
268 cv::Mat bgr_image = cv_ptr->image;
269 cv::Mat mask_image = mask_ptr->image;
270 cv::Mat masked_image;
271 bgr_image.copyTo(masked_image, mask_image);
272 sensor_msgs::Image::Ptr ros_masked_image
278 processBGR(bgr_image, mask_image, image->header);
279 processHSI(bgr_image, mask_image, image->header);
image_transport::SubscriberFilter image_sub_
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher i_hist_pub_
ros::Publisher s_hist_pub_
virtual void unsubscribe()
boost::shared_ptr< message_filters::Synchronizer< MaskSyncPolicy > > mask_sync_
ros::Publisher h_hist_pub_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::Subscriber< geometry_msgs::PolygonStamped > rectangle_sub_
ros::Publisher b_hist_pub_
virtual void convertHistogramToMsg(const cv::Mat &hist, int size, jsk_recognition_msgs::ColorHistogram &msg)
std::vector< std::string > V_string
virtual void processBGR(const cv::Mat &bgr_image, const std_msgs::Header &header)
ros::Publisher r_hist_pub_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::NodeHandle & getNodeHandle() const
PLUGINLIB_EXPORT_CLASS(jsk_perception::ColorHistogram, nodelet::Nodelet)
virtual void extract(const sensor_msgs::Image::ConstPtr &image, const geometry_msgs::PolygonStamped::ConstPtr &rectangle)
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
jsk_perception::ColorHistogramConfig Config
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
ros::Publisher g_hist_pub_
image_transport::SubscriberFilter image_mask_sub_
virtual void processHSI(const cv::Mat &bgr_image, const std_msgs::Header &header)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::Publisher image_pub_
virtual void extractMask(const sensor_msgs::Image::ConstPtr &image, const sensor_msgs::Image::ConstPtr &mask_image)
boost::shared_ptr< image_transport::ImageTransport > it_
void configCallback(Config &new_config, uint32_t level)
sensor_msgs::ImagePtr toImageMsg() const